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ABSTRACT 


Flexibility occurs in all mechanisms, to some degree. The 
dynamics of most flexible robotic manipulators can be modeled by 
a system of coupled ordinary differential equations. This study 
presents a general model of the dynamics of flexible robotic 
manipulators, including the gross motion of the links, the 
vibrations of the links and joints, and the dynamic coupling 
between the gross motions and vibrations. 

The vibrations in the links may be modeled using lumped 
parameters, truncated modal summation, a component mode 
synthesis method, or a "mixture" of these methods. The local 
link inertia matrix is derived to obtain the coupling terms 
between the gross motion of the link and the vibrations of the 
link. Coupling between the motions of the links results from the 
kinematic model, which utilizes the method of kinematic 
influence. 

The model is used to simulate the dynamics of a flexible, 
space-based robotic manipulator which is attached to a 
spacecraft, and is free to move with respect to the inertial 
reference frame. This model may be used to study the dynamic 
response of the manipulator to the motions of its joints, or to 
externally applied disturbances. 
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1.0 Introduction 


All real mechanical devices exhibit some flexibility. If 
the flexibility is negligible, the system dynamics can be 
modeled by a set of second order differential equations, 
developed from Newton’s laws of motion. But for other compliant 
systems, this model is not sufficient. The structural dynamics, 
and the coupling between the structural and gross motions of the 
system must be included to obtain a model of the system 
dynamics. 

Models of vibrations in continuous systems usually assume 
that the structure is homogeneous, isotropic, and obeys Hooke’s 
law within the elastic limit [23]. The dynamics of the system is 
represented by partial differential equations, such as the Euler 
equation for beams. This form of equation is not suitable for 
real-time computation of the dynamic model. Therefore mode 
summation methods, component mode synthesis methods, and lumped 
parameter methods are used. These methods use second order 
ordinary differential equations to model vibrations, and are 
sufficient to describe the vibrations in almost all flexible 
manipulators. 

The first goal of this study is to formulate a total 
model of the dynamics of a flexible manipulator system, which is 
valid for a wide variety of flexible robotic systems. The second 
goal is to analyze the dynamic coupling which occurs between the 
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gross motion and vibrations in the system. The final goal is to 
present specific applications of the model which utilize the 
knowledge of dynamic coupling to study the system dynamics. 

1.1 Literature Review 

Research in the dynamics of robotics and other spatial 
mechanisms is a union of geometry, the kinematics of mechanisms, 
and classical mechanics, each drawing on the knowledge of the 
other two. Robotics, as a broader subject, involves principles 
from almost every field of engineering and science. 
Researchers in the field have differing backgrounds, and 
therefore many methods and perspectives have been published 
about the various subjects in the field. This review will cover 
the major papers relevant to the dynamics of flexible 
manipulators. 

Benedict and Tesar [1-3] introduced the concept of 
kinematic influence coefficients, which were applied to rigid 
and quasi-rigid planar mechanisms. Sanders and Tesar [ 22 ] showed 
via experimentation that the quasi -static assumption is valid 
for mechanisms which operate well below the first natural 
frequency of the mechanism. The concept of kinematic influence 
was generalized for spatial mechanisms with rigid links by 
Thomas and Tesar [4]. Freeman and Tesar [5] showed that this 
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method is extremely powerful and may be used to model the 
dynamics of both serial and parallel robotic manipulators. 
Fresonki. Henandez. and Tesar [61125] used the concept of 
kinematic influence to obtain a description of the spatial 
deflections in robotic mechanisms. Kinematic influence was used 
by Behi and Tesar [7] to model vibrations in a multi -degree of 
freedom system due to flexibilities in the drive mechanisms. 
Wander and Tesar [8] proved that a totally general dynamic model 
of a robotic manipulator may be computed in real-time using the 
method of kinematic influence coefficients. 

Denavit and Hartenberg [9] are well know for a kinematic 
notation used to describe spatial, multi-degree of freedom 
devices. Most kinematics notational schemes seen in the 
literature vary little from this scheme. A recursive algorithm 
for computing the dynamics of rigid manipulators was presented 
by Hoi lerbach [10]. Book [11-15], Maizza-Neto [12,16], and 
Whitney, [12,15] and their associates were some of the first to 
study the dynamics of flexible manipulators. Book [14] 
formulated a recursive Lagrangian algorithm with a truncated 
mode summation representation of vibrating links, as did King, 
Gouri shankar. and Rink [22]. In [16], Hughes developed a model 
of the space shuttle manipulator arm. Hamilton’s principle was 
used by Low [17] to develop the explicit equations of motion for 
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a manipulator with flexible links. Finite element methods have 
been used by Sunada and Dubowsky [18], and by Naganathan and 
Soni [19] to analyze manipulators with elastic links. Huang and 
Lee [20] extended the Newton-Euler formulation of dynamics to 
model non-rigid manipulators. 


2.0 The Reference Model 

The dynamic model of a flexible manipulator presented 
here Is a general method which may be used to model the majority 
of flexible manipulators. Drive mechanism flexibilities may be 
modeled using lumped parmeters. Structural flexibilities may be 
modeled using lumped parameters, assumed modes ( a truncated 
mode summation technique), a component mode synthesis technique, 
or a combination of these methods. The result is a set of 
ordinary coupled differential equations. which model the 
dynamics of the system, including the gross motions, the 
vibrations, and the coupling between the gross motions and 

vibrat ions. 

All models of vibrations are approximations. Due to the 
generality of the model, and the variety of methods which may be 
used to represent the vibrations, it is possible to create a 
model which is a much closer approximation to the real system, 
than it would if only one method was used to represent the 
vibrations. To do so, the actual deflections of the system must 
be observed, or predicted to assure that the vibrations model 
is correct. The reader unfamiliar with models of vibrations in 
continuous systems is urged to refer to Thomson [23]. 
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2. 1 Geometry of a Flexible Robotic System 

In a spatial manipulator consisting of n+1 distinct 
links, a local body-fixed reference frame is assigned to each 
link. A requirement of Newtonian dynamics is that all motion 
must be measured relative to an inertial reference frame. If the 
base link of the manipulator is fixed relative to the inertial 
reference frame, the reference system of the base link becomes 
the inertial frame. Otherwise, a frame which is not attached to 
the manipulator will be the inertial reference frame. Thus, for 
the constrained case there will be n+1 reference frames, and for 
the unconstrained case there will be n+2. One of the reference 
frames is chosen to be the global reference frame, and is 
denoted as frame h, as shown if Figure 2.1-1. The global frame 
serves as a common frame to which all vector quantities will be 
referenced. Notice that the global frame is not required to be 
the same a^ the inertial frame. A preceeding superscript 
enclosed in parentheses is used to denote the local frame to 
which a vector is referenced, (ie. (1) R is referenced to frame 
i. ) If the superscript is not shown, the vector is assumed to be 
referenced to frame h. 

The geometry of the manipulator is described by the 
instamtaneous orientations and positions of the reference 
frames, shown in Figure 2.1-1. Frame i is attached to link 1. 
The unit vectors S*. S*. aind S* are the direction cosines of the 
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global frame h 


Figure 2.1-1 The Reference Frames 
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x, y, and z axes of frame i, defining the orientation. The unit 
vector S* coincides with the line of action of joint i. Joint i 
may be a revolute or prismatic Juncture between links i-1 and i. 
S* is a common perpendicular to the succesive joint axes and 
S , when no deflection occurs in link i. 

-i+l 

The undeflected position of a flexible link is considered 
to be the reference position. When link i is in its reference 
position, the direction cosines of local frame i+1 are denoted 

A I At At 

as S* , S* , and S* . The vector defines the position of a 
point on link i, relative to the origin of frame i. Deflections 
of the point are described by the vector functionals dCx^), and 
eCx^), which are the linear and angular deflections relative to 
the reference position. 

2.1.1 Angular Displacements and Deflections 

The relative orientation of succesive local coordinate 
frames is defined by a set of ordered rotations, shown in 
Figure 2. 1-2. The angular deflections of the link are assumed to 
be small, and thus add vectorial ly. This first order 
approximation of the rotation is necessary in three dimensional 
systems where there is no dominant angular deflection in one 
direction, and will introduce errors into the geometry 
calculations smaller than 5% if the magnitude of the angular 
deflections is less than 3 degrees. The angular deflection of a 
point on link i from its undeflected orientation is represented 
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Figure 2. 1-2 Angular Link Parameters 



Figure 2.1-3 Translational Link Parameters 


/ini 




Figure 2.1-4 Position Vectors 
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Figure 2. 1-5 


Local Link Deflections 




by the vector functional ©(x^ which describes the relative 
translational deflection of the distal end of link i from its 
reference position. Let g (j^) be the shape function describing 
the rotational deflection of mode J of link i, and be the 
corresponding generalized coordinate which is the magnitude of 
the mode. The functional relationship between the total angular 
deflection of a small element on a link with m modes is: 

i 

m 

“’SI*,) ■[{<!„ i,%> } (21-1) 

J*1 

or. in matrix form: 


<n §(x,) » ^(x^ J g t 


(2. 1-2) 


When the elements of the rotation are organized into a 
skew-symmetric form, the small rotation matrix results. 


[T0(x t )l 


1 -6 (x, ) 6 (x )1 

2 -i y -1 

e (x ) 1 -0 (X ) 

* "I X “i 

-0 (x ) 0 (x ) 1 

y -1 x -1 J 


(2. 1-3) 


These rotations are measured in the local 1th coordinate system. 
Notice that the determinant of this small rotation matrix is 

deUTOtx^] *1+| 0(x t )| a 1, (2.1-4) 

because | 0(^)1 is small, as assumed previously. The inverse of 
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the small rotation matrix can then be approximated as the 
transpose: 

[T0(x i )] _1 » [T0(x t )] T (2.1-5) 


Next, the angle is defined about the resulting x-axis. The 
new z-axis, formed by these rotations, is parallel to the 
S z axis, which is the line of action of Joint i+1. The final 
rotation is ^ about S^. where ^ is the sum of the gross 
displacement of Joint i+1. * l+1 - and the deflection of the 

Joint, 5« l . The resultant transformation from coordinate 
system i to coordinate system i+1 is: 


[ 


!♦!, 


V 


fl o 0 


[Toa^i 


0 c(a t ) -sCo^) 
0 sCc^) cto^) 


c (*; +1 ) " s( ^ 1+1 ) 0 
0 

0 o 1_ 


( 2 . 1 - 6 ) 


where s() * sineO and.cO 3 cosineO. 
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The matrix [r^] transforms a vector from the local coordinate 
system of link i to the coordinate system of the reference link, 
h. The columns of ["T^l are composed of the direction cosines of 
the unit vectors §*, S y , and §*, and are obtained via the 
f ormula: 


( h)gZ 

“I 


1 "I 

= n t J T J for i > h 

J=h 


- tl] (identity matrix) for i = h 


h-l 

* n l J T J for i < h (2. 1-7) 

J = t 


[“t j * r (h> s x i th, s y i 
i L -» - i 



15 


2.1.2 Translational Displacements and Deflections 

The reference position of the distal end of the link is 
L and is the sum of the vector a directed along S , and the 
vector S which is directed along S* , as shown if Figure 

-i+1 11 

2.1-3. The vector functional dt^) describes the relative 

translational deflection of the distal end of link i from its 

reference position. Let 3 (x^ be the shape function, or mode 

shape describing the translational deflection of mode j of link 

i, and q be the corresponding generalized coordinate which is 

the magnitude of the mode. The functional relationship between 

the generalized coordinates and the translational deflection of 

a link with modes is: 

j=i 


or, in matrix form: 

= [S(x t ) ] 3i (2.1-91 

The dimension of the modal matrix is 3xm^, and the columns are 
the translational parts of the mode shape functions. 
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The vector equations describing the position of a point 
on link i relative to the origin of link h (for a serial chain) 
are: 

FOR i>h: 

i -l 

R(x t ) = £ [ h T j ]| * J) L j + <J) d(L j )j + [“tj! ll) x t + (1) d(x)J 

J=h 

j*h 

* ‘"s, * [ 4 <S.) ] 3, } 

FOR i=h: 

R(X t ) * x ( j q i (2. 1-10) 

FOR i<h: 

5'S, 1 - I- * '% *[ ] 3 , } 

J *h-l 

+ t \ 1 { <l> 5 l + [ a( JS l ) ] 3 t } 

These vectors are shown in Figure 2. 1-4. 
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2.1.3 Position of the Center of Mass 

Also of geometric interest is the equation of the center 
of mass of the system. Let cm be the vector from the origin of 
link i to the center of mass of link i, in the reference 
position. Let ^ be the ratio of the mass of link i to the total 
mass of the system. For a rigid system, the position of the 
center of mass is: 


n j - i h- 1 h- 1 

?cm = I — l + l { 4 j( E ^ ] } " E { **[ \ ^ l l 

1 sO J =h+ 1 k = h J=0 k= J 


( 2 . 1 - 11 ) 


Although the derivation of this equation is not given here, the 
reader may verify it by noting that: 

11 n ^ 

B»I*. ‘ l{“, B l£ V } 

1=0 1=0 


( 2 . 1 - 12 ) 
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2.2 Kinematics of a Flexible Robotic System 

Kinematics is a study of how motions interact based on 
the geometric properties of a mechanism, ignoring the forces 
which may occur. The complete kinematic description of a serial 
chain of compliant bodies Includes the motion of the actuated 
joints, the motion due to vibrations, and the motion of the 
whole system relative to an inertial reference frame. 

Thomas, Tesar, Freeman, [4-51 and their associates have 
developed a general kinematic representation to describe 
actuated motion in rigid-body mechanisms, for both serial and 
parallel link topologies. Fresonki, Behl, and Tesar [6-7] have 
used the concepts developed for rigid body kinematics in a model 
of the kinematics of a compliant system, to predict the 
deflections and frequency response of the mechanism. 

In this model, the concept of kinematic influence is 
extended to ascertain the total kinematic effect of motions due 
to compliance of both Joints and links. The gross motion of the 
whole system relative to an inertial reference frame is also 
examined. The final result is a general kinematic model based on 
the kinematic Influence of all possible motions in a compliant 
system. 

The relative motion between bodies is defined by the 
relative motions of the local reference frames. Each motion is 
defined by a line of action, S, and a magnitude, 4 >. The line of 
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action of Joint i between links i-1 and 1 is defined by and 
the magnitude is defined by The motions due to the 

vibrations are defined by the three orthogonal unit vectors S*. 
S y , and S*. and by the mode shape and magnitude of each mode of 
vibration. 

The kinematic foundation used to model the spatial 
deflections of the system, and the motion of the system relative 
to the inertial reference frame, is similar to the kinematic 
foundation used to model the motion due to the actuated Joints. 
The original development of this kinematic method was performed 
by Tesar, and his associates [1-7] and was used to model 
mechanisms with no flexibility, which were fixed relative to the 
inertial reference frame. The degrees of freedom in these 
systems were always associated with an actuated Joint, and thus 
the term input became synonymous with the term degree of 

freedom. Behi , Fresonki, and Tesar extended this model to 
include deflections of the system. In order to emphasize the 
fact that the spatial deflections can be modeled in the same 

manner as the actuated Joints, the spatial deflections have been 

called pseudo-inputs. In this model, the spatial deflections of 
the link dtx^ and etx^, and the Joint deflection are 

pseudo- inputs. The pseudo-inputs are related to the generalized 
coordinates of vibrations via the modal matrix. 
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2.2.1 General Kinematic Foundation 

Many popular kinematic formulations are based on a 
recursive algorithm which transforms velocities in a serial 
fashion from local coordinate system i to i + 1, adding the 
contribution of local motions in an iterative procedure. 
Kinematics may be approached in a more efficient way by 
utilizing the geometric information of the system, allowing the 
velocity contribution of each motion to be computed 
independently. [8] 

The velocity of a point P on link i is the vector sum of 
the velocity contributions of all motions between the inertial 
reference frame and the point P. The velocity contribution of a 
single motion is the magnitude of that motion multiplied by a 
kinematic influence coefficient. The kinematic influence 
coefficient is a vector derivative, and is readily obtained from 
the geometric description of the mechanism. By organizing the 
kinematic influence coefficients into a matrix, the Jacobian is 
formed. 

Each type of motion which may occur, may be considered to 
be a relative velocity, and may be modeled by this method. The 
gross motion of the system, the joint motions, and the spatial 
deflections each are multiplied by an appropriate kinematic 
influence coefficient. The sum of all these relative motions 
gives the "absolute" velocity. 
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2.2.2 First Order Kinematic Influence 

The first order kinematic influence coefficient is 
defined as the rate of change of the position or orientation of 
a point with respect to a position state. A position state may 
be the gross position of the mechanism, the position of an 
actuated Joint, or the deflection of the link or joint. The 
"absolute" translational velocity of a point at position ^ on 
link k is the sum of the kinematic influence coefficients 
multiplied by the corresponding velocity states: 




•K 


dof 

8R(a k )/3v l *> t ] = [ [ 

1 = 1 




(2.2-1) 


where dof * the total number of degrees of freedom in the 
system, and gJCjjJ is the translational kinematic influence 
coefficient. The "absolute" rotational velocity is 


u(x ) 


* [ ( i. *. ] * l ( 


1=1 


1=1 



( 2 . 2 - 2 ) 


where g^( x^) is the 


rotational kinematic influence coefficient. 
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2. 2. 2.1 Translations 

The kinematic influence of a translational motion on the 
translation of a point P (in a serial mechanism) is the S vector 
describing the line of action of the motion. For a translational 
motion designated els $ , and a point P at local reference 
position x^, the kinematic Influence coefficient is defined by 
the following: 

IF the translational motion j causes point P to translate 
relative to the global frame h, AND k*h: 


(h) p. . 

s/sj 



(2.2-3a) 


IF the translational motion J causes point P to translate 
relative to the global frame h, AND k<h: 


"V<*, = - ,h, i 

°J Tc “J 


For all other cases, 


(h) p, , 

®j Tc = 9 


(2. 2-3b) 


(2. 2-3c) 
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The kinematic influence of a rotational motion on the 
translation of a point (in a serial mechanism) is the cross 
product of the line of action of the rotation, S, and a vector 
from the line of action of the motion to the point P. For a 
rotational motion at local reference position x jf and a point P 
at local reference position x^, the kinematic influence is 
defined by the following: 

IF the rotational motion j causes point P to translate relative 
to the global frame h, AND kah: 

lh, g P (x ) = (h, S x ( th) R(x ) - <h> R(x )}• (2. 2-4a) 

J J t J J 

IF the rotational motion j causes point P to translate relative 
to the global frame h, AND k<h: 

‘^g^xj = <h) S j x | <h) E(x j )- (h) R(x k )|- (2. 2-4b) 


For all other cases, 


(h) P 


s/s. 


= 0 


(2.2-4c) 
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2. 2. 2. 2 Rotations 

The kinematic influence of a rotational motion on the 
rotation of a differential mass element surrounding point P (in 
a serial mechanism) is the vector describing the line of 
action of the motion. For a rotational motion at local reference 
position and an element P at local reference position x^, 
the kinematic influence coefficient is defined by the following: 


IF the rotational motion J causes the differential element 
element surrounding point P to rotate relative to the global 
reference frame h, AND kfch: 


(h) R, x 

vv ■ 


<h>; 


(2. 2-5a) 


IF the rotational motion J causes the differential element 
element surrounding point P to rotate relative to the global 
reference frame h, AND k<h: 


(h) R, x (h)" 

V*K> ’ ' -j' 


(2.2-5b) 


For all other cases, 


(h) r, , 

V s *’ * 


(2.2-Sc) 


2. 2. 2. 3 Translations and Rotations Due to 
Motion Of The Global Frame 


Systems with bases which move relative to the Inertial 
reference frame undergo gross motion which must be included in 
the dynamics of the system, therefore they will be included in 
the kinematic model of the system. These motions are easily 
described as three orthogonal translations and three orthogonal 
rotations at the global frame. 


2. 2. 2. 4 Matrix Notation and the Jacobian Matrix 

The kinematic influence coefficients for a point P can be 
organized into matrix form (a Jacobian): 


(h, R(x i ) = [ (h) G P (x t )j £ 

and 

(h) i l (x 1 ) = [ (h) G R (x t )] k 

Or in a more compact notation, 


( 2 . 2 - 6 ) 


(2.2-7) 
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2.2.3 Second Order Kinematic Influence 

The second order kinematic relationships for a serial 
chain are defined by the second derivative of the position: 



r dof , , i 



R = d/dt « 

i ( *.] 

► 



L 1«1 J 



dof 

dof dof 

r 

-l[4 

’ *.] * i i 

4 

8 / 8 *,( 1 *,*j 

1*1 

1=1 j*l 

l J 


dof dof dof 

•E (<?.]♦ E I 

1=1 1=1 j = 1 


h <p (p 
-lj 


(2.2-9) 


The second order relationships of the rotations are : 


dof 

dof dof 

r 

- 

• r f r •• 

2 ■ l [ 8| *,J 

T 



L— J 
+ 

W a *> s" 

) V, 

1*1 

1=1 J=1 

L 


dof 

dof dof 

r 



+ 

t — 3 
C — 1 

A 

. R • • 

h <p & 

-ij 

► 

1=1 

1=1 J =1 

L J 



( 2 . 2 - 10 ) 


The second order kinematic influence coefficient is defined by 
the vector derivative of the first order kinematic influence 
coefficient. This relationship defines all centrifugal and 
coriolis acceleration terms which result from coupling between 
velocities due to the rotation of the local reference frames. 
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2. 2. 3.1 Second Order Translations 

The translational second-order kinematic relationship for 
a point P at reference position ^ on link k is defined by the 
following: 

IF the motions l and j are rotational and occur between the 
global frame h and point P, AND ksh: 


(h, h P (x ) = <h) S. x 

-II it i “ “ 


(2.2-lla) 


(where 1 is the minimum of i and j, and m is the maximum) 
IF the motion i and J are rotational and occur between the 
global frame h and point P, AND k<h: 


<h) h P (x ) = 
-lj -V 


- (h) S x (h> g P (x ). 

~1 a m Tc 


(2.2-llb) 


(where 1 is the minimum of i and J, and m is the maximum) 
IF the motion i is rotational and the motion J is translational, 
and i occurs between frame h and motion J, and AND k h. 


"V (X ) - °"S, X 'VlsJ (2.2-110 

\ j “Tc * J * 

IF the motion i is rotational and the motion J is translational 
and i occurs between frame h and motion J, and AND ksh: 


(h) h P (x ) = 

-lj “1C 


Jh % - (h, s^ ) 


(2. 2-1 Id) 
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For all other cases, 

(h). p , . , 

h ( 3 J = 0 (2.2-lle) 

Note that <h is always associated with a rotational motion, 

( h) P 

and may be associated with a rotational motion 

contributing to the translation of point P, or associated with a 
translational motion. 


2. 2. 3. 2 Second Order Rotations 

The rotational second-order kinematic influence 
coefficient for an element at reference position x on link k is 
defined by the following: 

IF the motion i is rotational and occurs between the local 
frame h and motion j, or with motion j, AND k*h: 


(h),R , 

h (x 

-1J -k 


(h) s X (h) - R 
“1 


•A 1 


(2. 2-12a) 


IF the motion i is rotational and occurs between the global 
frame h and motion j, or with motion j, AND k<h: 


(h), r , , 

h (x ) = 
-ij “v 


-(h>i 


(h) R, , 


(2.2-12b) 
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For all other cases, 


"V <*> 

-lj “k 


= 0 


(2.2-12c) 


Note that in this case, 
with rotational motions. 


(h) 


S and lh> Sj 311,6 alwa y s associated 


2 . 2 . 3. 3 Second Order Kinematic Effects Due to Gross Motion 

The gross rotation of the system can couple with the 
relative rotations and translations to cause coriolis and 
centrifugal accelerations, in the same way a relative motion 
does. The gross translations do not contribute to the 
the second order accelerations of the system. These kinematic 
formulas used to compute these kinematic effects are the same as 
those for the joints, noting that the gross motion occurs at 
the global frame, and therefore equations 2 . 2-lla. 11c. and 12a 


are used for all O^k^n. 
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2. 2. 3. 4 Matrix Notation and the Hessian Matrix 

The second order kinematic influence coefficients for a 
point P can be organized into matrix form, called the Hessian 
matrix. The acceleration can be written as: 


<h) 


R(x t ) = 


<h) r p 


G^x t ) 



(h) 


HP (x t ) 


3 


and 

w (x ) = G (xH f + <£ \ H (xH g 


( 2 . 2 - 13 ) 


( 2 . 2 - 14 ) 


In a more compact notation. 


<h, vv 


(h) 




[‘ h '«x,)]i * i , ['“HI Sl >]i 


( 2 . 2 - 15 ) 


The dimension of (h) H(x t ) is (dof x 6 x dof). 


2.2.4 Local Link Kinematics 

The velocity of a point on a compliant link, relative to 
the local reference fraune, is defined by a vector function of 
the mode shapes and the time rates of change of the 
corresponding generalized coordinates. This information 


describes the rotational and translational deflections in the 
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three orthogonal directions. The mode shapes may be a function, 
or a set of data obtained via modal analysis or a finite element 
simulation of link deflections. For a lumped parameter model, 
the mode shapes are assumed to be linear functions. The 
direction of the motion due to vibrations between reference 
frames 1 and i+1 is defined by the lines of action in the three 
orthogonal directions, S*. §*, and S*. Note that these motions 
take place at the distal end of the link, coinciding with frame 
i+1, but are measured in local frame i. 

2. 2. 4.1 Local Link Velocities and Accelerations 

The first order relationship which defines the spatial 
translational and rotational deflectional velocities of a point 
associated with the reference position <l> Xj. is : 

“’d^) * [ “W,) ] g t (2.2-16) 

(,, 0 t ( Si ) = [ ] g t (2.2-17) 

This simple relationship results from the assumption of the 
vibrations model that the mode shapes are time invarient. The 
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form of the second order relationship is similar to the first 
order relationship: 

= [ “’s/x,) ] 5 t (2.2-18) 

<i> 0 i (^ ) = [ “V/*,) ] 5, (2.2-19) 

The velocity of the end of the link is the velocity of the point 
at (l) ^ l 3 (1) -i' Note that the kinematic influence of the 
vibrational motions are represented as three translations and 
three rotations at the distal end of the link, but m ^ degrees of 
freedom are needed to describe the dynamics of these 
translations and rotations. The velocities of the spatial 
deflections contribute to the absolute velocities via the 
appropriate kinematic influence coefficients, which were defined 
in the previous sections. 

2.2.5 Kinematics of the Center of Hass of the System 

It is not nessesary to formulate the dynamics of the 
system In terms of the center of mass of the system, although it 
may be done. It is the opinion of the author that such 
formulations are unneccesary for the general model of dynamics, 
and only make the problem more complicated and computational 
Inefficient. The only kinematic information needed to complete 
this kinematic model is the velocity of the center of mass of 
the system, relative to the global frame. Note that this 
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information is not required for the dynamics model, but may be 
needed for control purposes. Defining to be the ratio of the 
mass of link i to the mass of the total system, and cn^ to be 
the vector form the origin of frame j to the center of mass of 
frame J. The kinematic effect of the rotational motion ^ on the 
translational velocity of the center of mass is: 




(R ) = 

-cm 




for i>h 


for ish 


( 2 . 2 - 20 ) 


These kinematic parameters give the translational velocity of 
the center of mass of the system relative to the origin of the 

global frame. 

2.2.6 Kinematic Models and Momentum Conservation 

If no external loads are applied to a system with a base 
which is free to move relative to the inertial frame, 
conservation of momentum may be used to derive a first order 
model of the system, based on the kinematics of the system, and 
the mass ratios of each link. A system of control based on these 
equations may be formulated. It can be shown that there is no 
obvious advantage to computing these equations using the center 
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of mass 

formulation, since 

the 

necessary 

equations 

of the 

momentum 

of the system can 

be 

obtained 

from the 

original 


kinematics. The reader with an intimate knowlege of dynamics 
should note that momentum of the system must be calculated with 
respect to the inertial reference frame. Any computation of the 
motion of a point in space or on the mechanism, based on the 
equations of momentum of the system, does not require that the 
motion of the center of mass be directly computed as an 
intermediate step. Therefore, any control scheme based on 
conservation of momentum does not require kinematic formulation 
in terms of the center of mass of the system. 
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2.3 The Dynamics of the System 

A complete model of system dynamics includes the coupling 
between gross motion and vibrations. Dynamic coupling terms 
originate from the kinematics of a system. By measuring joint 
angles relative to the preceeding link, off-diagonal terms are 
produced in the rotational Jacobian, and in the global Inertia 
modeling matrix for the system. Dynamic coupling will occur in 
all systems where states (ie. displacement, velocity, ..) are 
not measured directly from the inertial reference frame. 

One of the most interesting, yet subtle examples of 
coupling is that between the vibrational modes of a link and the 
gross motions of the link. Most models ignore this coupling by 
assuming that the off-diagonal terms of the local link inertia 
matrix, which correspond to this coupling, are zero. These 
coupling terms, and all other inertia terms, can be derived from 
expressions of the systems kinetic and potential energies. 
Lagrange’s Equation is then used to produce the equations of 
motion, resulting in one equation for each degree of freedom of 
the system. For a system in space (unconstrained), there there 
are six degrees of freedom for the gross motion, one degree of 
freedom for each joint, and one degree of freedom for each mode 


of vibration. 
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2.3.1 Vibration Model Concepts 

Vibrations in the system are described by the normal 
modes, which exhibit harmonic motion at the corresponding root 
frequency. In theory, there is an infinite number of modes for a 
continuous body, such as a beam. But in a real system, this 
theory is not totally accurate in predicting the actual modes, 
because the assumptions of the theory are only approximations of 
reality (ie. the beam is not a true continuous system, and 
material imperfections are not modeled). Also, higher modes 
usually do not have a measureable effect on the dynamics of the 
real system, because any large amplitude vibrations at higher 
frequencies die out quickly due to structural damping. 
Therefore, the inertia and stiffness terms used in the system 
model should be based on experimental data obtained from modal 
analysis, and metrology, or predicted from computational methods 
such as finite element analysis. 

The equivalent mass and stiffness of a link can be 
obtained by experiments in modal analysis, and other forms of 
metrology. But unfortunately, these methods do not reveal the 
inertia terms which describe the coupling between the gross 
motions and vibrations in the link, which are off-diagonal terms 
in the Inertia matrix. These terms can be derived from the 
kinetic energy of the system, and a method of predicting their 
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magnitudes is presented in this study. 

To predict the inertia terms of the system, a mode 

summation method is used where the mode shapes and mass 
distribution of each link are assumed to be known. This 
information must be obtained via ekperlmentai or computational 
methods. An accurate knowledge of this information assures that 


the dynamic model will be accurate. These mode shapes can be 
represented by any function or set of data. A finite number of 
modes are used. Lumped parameter models are considered to be a 
special case, where the mode shapes are simple linear functions 
and all mass is lumped at the center of mass of the link. To 
accomodate more flexible links, a link model similar to 
component mode sythesls Is used, allowing the link to be 
subdivided into smaller sections, or link segments. The 
rotational and translational deflections of a sub-link are 
assumed to obey the magnitude constraints Imposed by the 
geometric model of rotations. 

There is always a question about when to use lumped 
parameters, mode summation, or component mode synthesis. No 
absolute guidelines can be presented as to when each method 
should be used. As a rule of thumb, lumped parameters should be 
used to model Joint flexibilities and short, fat links. Mode 
summation should be used to model longer links which exlbit 
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angular deflections of less than five degrees. Component mode 
synthesis should be used to model flexible links which have 
angular deflections greater than five degrees. Note that these 
models may be "mixed" together in the overall system model. 

A firm understanding of the assumptions inherent in these 
modeling methods is a prerequisite to insuring that the model 
represents the actual system. It must be stressed that mode 
summation methods require the beam to be relatively long and 
thin, but the cross sectional area is not required to be 
constant. All models of vibrating systems are approximations of 
very complex phenomena. A good model will not oversimplify the 
problem. On the other hand, a model should not Include more 
information than is necessary, due to the computational burden 
imposed. Most importantly, it must be realized that the quality 
of the information used will have a great effect on the accuracy 
of model. A qualitatize knowledge of the relationship between 
accuracy of the data used in the model to the accuracy of the 
results obtained is absolutely necessary. 
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2.3.2 Kinetic Energy and the System Inertia Matrix 

The total kinetic energy of the system is the sum of the 
kinetic energies of its sub-systems. Each sub-system can be a 
set of links, one link, or part of a link. Although this is not 
required, it is convenient to choose each sub-system so the 
local flexibility matrix is constant. Therefore, for lumped 
parameter models and mode summation models, a sub-system will be 
one link, and for component mode synthesis models, a sub-system 
will be a link segment. The kinetic energy of an element of 
mass, 5m, in subsystem i will be 


5KE t = | 5m | R(x t ) | | Rix^) j (2.3-1) 

where R(x t ) is the absolute velocity of the element on link i. 
The total kinetic energy for the sub-system is the sum of the 
kinetic energies of all mass elements. Assuming the mass 
distribution of the sub-system is known, the kinetic energy can 
ideally be expressed as a volumetric Integral: 


KE = ~ 
1 2 


p(x) { R(Xj ) ]• • { S(x t ) } dV t 


(2.3-2) 


R(x ) can be expressed as the velocity of a reference point plus 
the velocity relative to this reference point. By, choosing the 
reference point to coincide with the local reference frame 
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origin of the link, the inertia terms commonly associated with 
the gross motion of the link are obtained, along with the 
equivalent masses of vibration, and the inertia coupling terms 
between the gross motion and vibrations. The detailed derivation 
of the kinetic energy and the inertia terras is presented in 
Appendix A. 

The kinetic energy of the link can be rewritten in terms 
of the local inertia matrix, the gross motions, and the 
velocities of vibration for the link. 

KE = i [ <‘>l ] < l >* 

I 2 r l L ! J r t 


u UM 
where g 


( 1 > R(Q i ) 

* 1 ) ■ 


(2.3-3) 


l J 

( 1 ) * 

and £(0^ is the translational velocity of the reference 
frame, (1> w(0 ) is the rotational velocity of the reference 
frame point, and g^ are the generalized velocities of vibration 


of link i. 
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The local velocities can be expressed in terms of the 
global reference frame, h, by multiplying by the rotation 

matrix. 


(h) • 

~ 

(h) R(0 i ) 

(h> tf(Qj ) 

• 

= 

[ h T i l U) R(Q i ) 

* * 

[ fc T i l <l> S (O l ) • 

• 


9» 


9 t 


[“t 1 0 0 

0 (“tj o 


ll> ?(— 1 ) 
‘‘’wCOj) • 

0 0 [I] 


« 

9, 

V. 


[I] is the m ( x m^ Identity matrix, and 
augmented transformation matrix. Rewriting the kinetic energy, 

B, * | 1 

m i] [\][“\] [\ 

= i (h) g T [ <h, I ] (h) ^. (2.3-5) 


M 


is the 
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At this point. It is necessary to express the kinetic 
energy In terms of another set of states which consists of the 
gross motion of the entire system, the velocities of the 
actuated inputs, and the velocities of all vibrations (le. at 
the joints, and in the links) in the system, <£. The kinematic 

• (h)» 

relationship between <£ and £ * s: 


<h> £ t * [ a<h> ^ l / a £ ] f * [ lh>f 1 ] k (2.3-6) 

where j is the Jacobian for the origin of local 

coordinate system i augmented by an identity relationship which 
specifies which generalized velocities of vibration are 
associated with this link: 




th) G (0) 


r <h, y i - 


i * 

• a 


L » J 


a 9,/ a 2 



(2.3-7) 


Substituting this expression into the kinetic energy, 


ICE , ■ ; ]>,]['"■,] [X fh, ] 

- | £ t f[ “i, ] ['“t. ] <• 


1 *T r (h>» n • 

] * 


(2.3-8) 


where [ <h) I* ] the the inertia modeling matrix for sub-system 
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i. The total kinetic energy of the system is the sum of the 
kinetic energy of each sub-system 
n + 1 

KE ■ 7 KE 

system I 

•”[ 1 s T [ “it ] i ,2 - 3 ' 91 

1 =i 

1 *T r (h) T * -1 • 

= -<£ [ I J 2 

where [ (h) I* ] is the inertia modeling matrix of the entire 


system. 
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2.3.3 Potential Energy and the Stiffness Matrix 

The potential energy due to compliance in the system is 
usually a linear function of the generalized coordinates of 
vibration. The potential energy of a differential element of 
link i can be expressed as 


<5PE 

i 


- S(EI) 
2 


{ais,) } • {ids,) } . 


(2.3-10) 


and Ideally may be represented as a volumetric integral over the 
link 


PE 

i 


1 

2 


EI/VCx^ ) 


| d ( x^ ) | | d ( x ^ ^ dV^ 


V 

i 


(2.3-11) 


A detailed derivation of the potential energy and stiffness 
terms is presented in Appendix B. 

The potential energy of the link can be rewritten in 
terms of the local stiffness matrix and the generalized 
coordinates of vibration for the link: 


PE , - < [ * J a, • 


(2.3-12) 
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Noting that the flexibilities may be extracted from the state 
position vector via an identity relationship defined as 

T /a I (2.3-13) 

g t = [ a 9 t / a 2 J 2’ 

the potential energy can be written In t.ras of the system state 
vector, 

PE, * t [ f C K , ] [ ] * ' 

T r/ 1 „ (2.3-14) 

- 8 [ K ! J 2 


where [ K* ] is the flexibility modeling matrix of link i. The 
total potential energy of the system Is the sum of the potential 

energy of the links. 


n + 1 _ m 

■IU 

i=l 

- | « T [ K * 1 * 


where [ K* ] is the flexibility modeling matrix of the entire 


system. 
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2.3.4 The Equations of Motion and the Power Modeling Matrix 

The equations of dynamics are obtained via Lagrange’s 
equation. For a multi-degree of freedom system, it can be 
expressed as: 


t= d/dt 3KE/aj T J- 3KE/3j T + d?E/df . (2.3-16) 

The term dKE/df T can be readily obtained due to the quadratic 
form of KE. 


3KE/a2 T - i [ (h) I* ] j 



1 *T 

; 2 


a / a » T [ (h) i* ] t 



(2.3-17) 


noting that [ <h) I ] is symmetric, and not a function of 

velocity. The time derivative of this term is 


d/dt 3KE/3g T 


* d/dt j[ <h) I* ] j | 


= { d/dt [ (h) I* ]| f ♦ [ (h, I* ]* . 


(2.3-18) 
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The time derivative of the inertia modeling matrix can be 
written as 


d/dt [ <h) I ] * <£ | d/d$p[ <h, I ]j • 


(2.3-19) 


where the planar transpose operator performs the following 
operation*. 


{i/dsn" 


(2.3-20) 


for all i. J, and k, where the notation is piane;rov,coiu»n. 


The next term of Lagrange’s Equation Is 
3KE/3{ T = d/df | \ f [ (h>1 ] £ } 

= | i T { »/»*[ "”i‘ ) } * ■ 


The last term yields 


3PE/3£ T = 3PE/3f T |^2 T [ k ] £ } 


“ [ K* ] f 


(2.3-22) 
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The system of equations can be written as 


r(h) T * -1- ^ *T 

I * [ 1 J 2 + 2 


{ a/d^ M i m ]}”- a/a t [ 


These terms can be written in a more compact 
matrix with dimensions (dof x dof x dof): 

[ (h, p* ] * | a/aj[ <h) i* ]}”- | | a/a f [ (h, i* ] }, 

so the dynamic equations can be written in a 
notation. 


i= ]£ + f T r h, p’] i* [t] 


• — 


(h) r 


Recalling the expression for [ <h) I ], 




r r J m f r«» p 1 T [ 

1*1 L J 


(h) T * i \ ♦ 

1 J } * 

C K ‘ Is ' 

(2.3-23) 

matrix form as a 
(2.3-24) 

standard matrix 
(2.3-25) 


(2.3-26) 
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The term \ d/df [ (h) f ] can be expressed as 

- T ; -** { [“». I'C 1 [' h ‘ s . ] } 

1=1 V. L -I 

- 1 i ** { [ <m ’, n [ <hi1 . i i 

1=1 ^ u 

*"[ i ] T { t ] 

1=1 L J ^ 

♦T i [“*, ] T c l “' i 1 1 ] } 

1=1 L J 

Notice that the second order kinematic influence 
are def i ned by 

] - [“*, ] • 

Substituting the local inertia matrix referenced 
frame, 

a/a E [ ] -a/a* [\ f} 

»a/a E {[\]}[ m i,] [\ ]’ 

♦ [“»,]•/•» {[“’i, ]}[*». f 
♦[*»,][ "' I , ] s/a * { [ ]1 


( 2 . 3 - 27 ) 

coefficients 

( 2 . 3 - 28 ) 
to the local 


( 2 . 3 - 29 ) 



50 


where 



1} 


will 


be denoted henceforth as 


[a a V«t]- 


Recalling the exact form of the augmented transformation matrix, 

[ h T t l 0 0 

0 [ h T i J 0 (2.3-30) 

0 0 [I] 

The columns of the tranformation matrix, ], are made up of 
the direction cosines of the ith local coordinate frame. These 
direction cosines are a function of the rotational dispacements 
between the global reference frame and the local frame. This 
kinematic relationship can be expressed by the cross-product of 
the first order rotational influence coefficients and the 
direction cosines of the local ith frame. This results is the 

second-order relationship: 


d/d<£ 


■M 




a/a <£ j [“tj = 


[ (h, Gj k ] x[\] I 
* 5 J * ♦ * 


t < h > G Jkj ( hr j , 
i t J i ; 2 


["Vi 



(2.3-31) 
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The skew-symmetric form of l <h) c| k ] may be substituted for the 
cross product, yielding 


d/df j t h T 1 1 = 

[t" ,i =! k '. J ‘\v i \\*] 


( 2 . 3 - 32 ) 


or, in more compact notation, 


a/a 4) t\l - ['“Bfl.j t-r.l 


( 2 . 3 - 33 ) 


Now, the term d/d<£ 


*".,{[•>,])■ 


,UM) 


can be expressed as 


[ (h, G Jk ]., 
1 » J 

0 

0 


0 
0 

0 o 


[ (h, G Jk ]., 
1 * J 


['.1 


( 2 . 3 - 34 ) 
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Letting the matrix of skew-symmetric forms be 


(MjsJk" 

t 


[ (h) G Jk ] . 0 


i ; i 


0 

0 


[ ( h)gjk] 


[ (h, G Jk ] 


0 

0 


» ; i 

0 0 


i ; 2 


0 

0 


[ <h, G Jk ] 


0 

0 


i ;2 
0 0 


(h, c; k ]. n 0 0 

0 [ (h, G Jlt ] 0 

t ; n 

0 0 0 


(2.3-35) 


the derivative in question cam be expressed in a much more 
compact form: 


3'S t {[“!,]}.[ r ^ ] 


(2.3-36) 



53 


The final resulting equation for the term a/a 2 [ (h) I ] Is a 

function of the local inertia tensor matrix and the geometry of 

the system: 


s/a* [‘"T ] - “£ 

1=1 

[“«. rr-. i 


* [•“», ] T ['“»r] [\]r\:i[\ ] T ["” s .] 

.[■“*. n “?r r*. i 


* f[\ ][« ,i, i/»d[x rr. ] 


( 2 . 3 - 37 ) 


It has dimensions of (dof x dof x dof). 
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2.3.5 Applied Forces and Torques 

The applied torque vector x is the equivalent load of any 
point loads or distributed loads which may be applied to the 
system. The equivalent load which corresponds to an applied 
point force and point moment at position x t on link 1 is: 

-equivalent [ ^-1^ ] -applied (2.3-38) 

The equivalent load expression for a distributed load is 

r <w *(x ) Yst 
L _1 J - ®pp ! 1 

which is a volumetric integral of the distributed load over each 
link, or sub-system. This is particularly important for an 
unconstrained system in a gravity, or pressure field. For 
orbiting systems, a slight differential in the gravity gradient 
field will cause a torque on the system. For underwater systems, 
a similar effect will occur due to the variation of water 
pressure with depth. 
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This gravity gradient effect can be estimated by assuming 
the distributed load to be a point load at the center of mass. 
This leads to the equation 


T a 

— equi valent 


l ( [ (K ' 9( *v ] t 


1*0 


-approx_t 


( 2 . 3 - 40 ) 


This same method can be used to estimate the equivalent loads of 
an underwater system, systems which develop fluid drag, systems 
working in a centrifuge, or systems in a gravity field. 

Damping terms have not been presented explicitly in this 
study, but may be modeled as loads which are functions of the 
magnitude of the vibrations, or the velocity of the vibrations. 
These loads are assumed to occur " Internal ly\ and are written 
in terms of the states of the system, f, and <£. Therefore it is 
not necessary to use the Jacobian to transform the damping 
loads, as it is with externally applied loads. 



3.0 Dynamic Simulations: 

The product of this research is a general model of the 
dynamics of serial manipulators. The model derived has been 
implemented in a simulation package in "C" (a programming 
language) which currently resides on a Silicon Graphics 4-D 
computer system at the University of Texas Mechanical Systems 
Robotics Laboratory. The source code for this program, called 
VSim, can be found in Appendix C. 

VSlm is a general simulation package, which can simulate 
serial systems of n links. Each link can have mt translational 
vibration modes and mr rotational vibration modes. The number of 
modes may differ from link to link (le. link 2 may have 1 
translational mode and 5 rotational modes, while link 1 may 
have 3 translational modes ...). A link may be modeled as a rigid 
body by assigning it no vibrational modes. Extremely flexible 
links may be subdivided and modeled as several sub-link. Each 
sub-link is modeled the same as a regular link. Each sub-link 
may be modeled with several modes. 

Because of the generality of the program, it must be 
stressed that the output is only as good as the input. If the 
input data truely represents the real system, there will be a 
good chance that the output data also represents the real system. 
The input to VSim includes geometry, inertia, and stiffness data 
for each link, along with a variety of “bookkeeping" data. In 
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order to promote efficiency in computations, yet preserve the 
general capabilities of the program, the current version requires 
that the input data be included in the file "Robotdat . c" . The 
control model must be included in the file "Model, c" which is 
accessed by the integration routine. The program generates the 
equations of motion internally, from the data provided, and from 
the control algorithm provided. The numerical integration 
algorithm uses a predictor-corrector method with adaptive 
step size and adaptive order, which was presented by C. William 

Gear [24]. 

3.1 Example: A Large Space-Based Robotic System 

As an example, the dynamic equations of a large flexible 
robotic system used in space operations are formulated, and the 
resulting system is simulated under various disturbing inputs to 
the end-effector and Joints. The system consists of a large 
spacecraft with a 55 ft. manipulator which has 6 actuated 
joints. The system was modeled such that the base link, (the 
spacecraft) could translate relative to the inertial reference 
frame. The geometry, mass, and stiffness information used for 
this example is similar to that of the Remote Manipulator System 
used by NASA. A truncated mode summation method will be used to 
model translational vibrations, and a lumped parameter method 


will be used to rotational vibrations. 



Table 3. 1-1 

Geometric Data for Example 



*£l CM 


Joint type 



revolute 


revolute 


revolute 


revol ute 


revolute 
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Links 2 and 3 were modeled as flexible, and the other 
links were considered to be rigid bodies. The translational 
vibrations transverse to the length of the link were modeled by 
cubic modes, and the torsion about the minor axis of the links 
were modeled by simple linear rotational springs. The inertia 
terms were computed using the equations derived in Appendix A. 

The translational cubic mode shapes are: 




a +b(x/L)+c(x/L) 2 +d(x/L) 3 

l ill ill ill 


(3. 1-1) 




a + b (x /L ) + c (x /L ) 2 + d (x /L ) 3 
2 2 11 2 11 2 11 


(3. 1-2) 




0 
0 

a + b(x/L)+c(x/L) 2 + d(x/L) 3 

3 3 11 3 11 3 11 J 


(3. 1-3) 


and 




0 

0 

a +b(x/L)+c(x/L) 2 + d(x/L) 3 
4 4 11 4 11 4 11 


(3. 1-4) 
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The proximal end of the link is not allowed to defect relative 
to the local coordinate system, 

s j (g i ) = 5 j (g i ) = | g j < 3 - 1_5) 

therefore a = 0. The slope of the deflection is also assumed to 
be zero at the local coordinate system, therefore = 0. 

The orthogonality condition needed to assure that these 
modes are independent is expressed as 



0 


(3. 1-7) 

Integrating, we find the orthogonality requirement to be 


r = -(42 + 35 r ) / (35 + 30 r ) (3.1-8) 

2 l 1 

where r = d /c . and r = d /c . For this example, the first 
1 11 2 2 2 

ratio is r =0, making the second ratio, r 
1 * 


= -1.2. 



IOj 109 
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The mode shape functions relating the local deflections 
to the generalized coordinates of the local link sire: 


w ■ 


‘VV 


(3. 1-9) 




(x^L^ 2 - 1.2 (x ( /L ( ) 3 


(3. 1-10) 


3 ( ^l ) 


0 

0 


(3. 1-11) 


(x ) = 

4 -i 


0 

0 

(x /L ) 2 - 1.2 (x /L ) 3 

i 1 11 


(3. 1-12) 


These mode shapes can be organized into matrix form which gives 
the local link translational deflections. 


dCx^) = £(>0 g i 


(3. 1-13) 


5 (x ) I 6 (x ) I 5 (x ) I 6 (x ) I 0 
-1 -1 -2 -i -3 - 1 -4 -1 - 


'll 


‘21 


31 


41 


^S1 
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The rotational mode shapes are assumed to be simple linear 
functions, which is the prescribed assumption for lumped 
paramter models: 



x /L 
i 1 

0 

0 


( 3 . 1 - 14 ) 


and may be organized into matrix form to obtain the local 
rotational deflection. 


ec^) 


0 I 0 I 0 I 0 1 sft (x ) 



( 3 . 1 - 15 ) 


The shape functions will now be used to compute the coupling 
terms and vibration terms of the local inertia matricies. 
From Appendix A, the inertial coupling between the translational 
flexibilities and the translational motion of the link are 




M 1 

1 M 1 

M 1 

M 1 

I 0 


-dll 

-dl2 

— d!3 

— dl 4 



such that 


( 3 . 1 - 16 ) 


M 


-dij 


' I 



( 3 . 1 - 17 ) 



64 


Assuming that the mass of the link is distributed evenly along the 
length of the link, such that the mat rix becomes 


0 0 

0 0 

cL/3 c L/30 

1 i 2 i 

For link number 2 this matrix is: 



0 

0 

0 

(3. 1-18) 



0 

3. 18 c 

l 

0 


0 

.318 c 

2 

0 


0 

0 

3. 18 c 

l 


0 

0 

.318 c 

2 


and for link number 3 this matrix is: 


0 

0 

0 


(3. 1-19) 



0 

1.99 c 

l 

0 


0 

. 199 c 

2 

0 


0 

0 

1.99 c 

l 


0 

0 

. 199 c 

2 


0 

0 

0 

(3. 1-20) 


The coeffients c and c are scale factors. 
1 2 



The coupling terms between the gross rotation and the 
translational flexibility are presented in Appendix A as: 
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[l p ' 


r m" i 

i if i 

m” 1 

m** i 

ol 

(3. 1-21 



L -Ldll 

-Ldl2 

-Ldi3 

-Ldi4 

- J 



such that 


m** = r p(x ) 

-Ldll J -i 

V 

[ *,] 5/5, 

) dV. 


(3. 1-22) 

The resulting matrix is 






0 

0 

0 

0 0 


[ C, ] ■ ■*. 

0 

0 

- c L 2 /4 
1 1 

- c l 2 /ioo 0 

2 i 

n 


- cL/4 
l l 

c L /100 
2 1 

0 

0 






(3. 1-22) 


The coupling 

between 

the rotational 

vibrations and 

the 


rotational gross motion was presented in Appendix A. 


f i T 1 


[ 0 1 

1 0 ! 

1 9 1 

1 0 1 

1 m L 1 

(3. 1-23) 

L ra ’ 1 . 


L - 




- Ld is J 



This matrix becomes 


0 

0 


0 0 1/2 
xxi 

0 0 0 
0 0 


0 


0 

0 

0 


0 


(3. 1-24) 



By combining these matricies we obtain the total coupling 
between the vibrations and the rotational gross motion. 

For link number 2 this matrix is 
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K]- 


o 

o 

49.8 c 


0 

0 

1.99 c 


0 

-49.8 c 


and the for link number 3 this matrix is 


0 1. 19 

-1.99 c 0 

2 

0 0 

(3. 1-25) 


M- 


o 

0 


0 

0 


0 0 . 585 

-34.6 c -1.39 c 0 

i 2 


34.6 c 1.39 c 

i 


2 0 0 

(3. 1-26 ) 

The generalized inertias of vibration are defined in Appendix A 
as 


C * J <>(*,) «/*,> •§„<*,) i*. 
1 


(3. 1-27) 


and for rotational vibrations it is given as 


i" = f I(x ) * <x) • 0 Cx ) 

qq J -t E J -1 E k -1 
1 J* •/ 


dV. 


(3. 1-28) 


The results of this integration may be organized into a 
matrix for each flexible link. 
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2.38 c 2 
0 
0 
0 
0 


0 

.05449 c 2 
0 
0 
0 


0 

0 

2.38 c 2 
0 
0 


0 o 

o 0 

o 0 

.05449 c 2 0 

z 

0 .793 


(3. 1-29) 



1.45 c 2 

0 

0 

0 

0 

1 

0 

.03419 c 2 

0 

0 

0 

0 

0 

1.45 c 2 
1 

0 

0 

0 

0 

0 

.03419 c 2 
2 

0 

0 

0 

0 

0 

.39 


(3. 1-30) 
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The stiffness terms for these two links are obtained from 
the equations presented in Appendix B. The resulting stiffness 
matrix for link 2 is: 


0 0 

0 0 

-3. 5e3 c c 0 

l 2 

7. 7e3 c 2 
! 2 

0 


and for link 3: 


c c 

l 

c 2 0 
2 

8. 3e5 


(3. 1-32) 


This system was simulated with various disturbances 


applied to the point of resolution (The point of resolution is a 


point at the end-effector of the manipulator). In the first 


simulation, a step load of 1000 lb. and 1000 ft-lb. was applied 


along each axis of the global frame. The simulation was 


performed once for configuration #1, for which all Joint 


displacements are zero, and once for configuration #2, for which 


all Joints are zero, except for Joint 3, which was set at n/2 


M 


2. 2e3 c -1 . 7e3 c c 0 

1 V 

-1. 7e3 c c 3. 8e3 c 0 

i 2 2 


0 

0 


0 

0 

0 


0 

0 

0 


2. 2e3 c -1. 7e3 

l 

-1 . 7e3 c c 3. 8e3 
1 2 


0 

1. Ie6 
(3. 1-31) 


0 

0 

0 


N- 


4.5e3 c -3.5e3 c c 

l 12 

“3. 5e3 c c 7. 7e3 c 2 
1 2 2 


0 

0 

0 


0 

0 

0 


0 
0 

4. 5e3 c 2 

l 

-3. 5e3 c c 

l : 
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radians. The base was allowed to translate. The magnitudes of 
the first and second translational modes for each flexible link 
are shown in Figures 3.1-la through 3. l-4b. for reference 
position *1. The translational modes are dominated by 
frequencies around 3 Hz. The impulse response of the rotational 
mode was found to be much faster than that of the translational 
modes. This is because of the small inertia of the system in 
configuration #1. The rotational vibrations of the system near 
configuration *1 will be dominated by the loadings at the 
end-effector. If a payload with large inertia is being moved, 
the frequency of the rotational vibrations will be lower. The 
simulation for configuarion #2, shown in Figure 3. l-5a through 
3. l-8b, shows the configuration dependence of the system 
response. These simulations reveal a much lower frequency on the 
order of 0.1 Hz, which modulates the amplitudes of the higher 
frequencies of vibrations. 

In the second simulation, the load was a cyclic 
disturbance of 100 lb and 100 ft-lb. along each axis of the 
global frame. The frequency of the load was 3.18 Hz. The 
simulation was performed once for reference position *1, for 
which all Joint dispacements are zero, and once for reference 
position #2, for which all Joints are zero, except for Joint 3, 
which was set at n/2 radians. The base was allowed to translate. 
The position of the end effector is shown in Figures 3. l-9a and 
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3. l-9b, for reference position #1. The magnitudes of the first 
and second modes for each flexible link are shown in Figures 
3. l-10a through 3. l-13b, for reference position #1. The 

magnitudes of the first and second modes for each flexible link 
are shown in Figures 3. l-14a through 3. l-17b, for reference 
position #2. 

The third set of simulations were performed for an 
oscillating load of 1000 lb. and 1000 ft-lb. , at a frequency of 
.318 Hz. The results for the magnitudes of the first and second 
modes for each flexible link are shown in Figures 3. l-18a 
through 3. l-18b. for reference position #1. The results for the 
magnitudes of the first and second modes for each flexible link 
are shown in Figures 3. l-22a through 3. l-25b, for reference 
position #1. 
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Figure 3. l-2a Link 1. Mode 3 time (seconds) 



Figure 3. l-2b Link 1, Mode 4 time (seconds) 
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Link 2, Mode 2 
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Figure 3. l-5b 


Link 1, Mode 2 


TIME (SECONDS) 


8 >87.-01 


* -8.ov-oi 


-1.79rH)0 


-2.7Sr*00 


-3.78.-fOO 

O.C 


Figure 


-7. 88.-01 


-1 . 47.-KX) 


-2. 1Sr*00 I 

0 . 0.+00 

Figure 3. 1-6 






u 

a 

o 


9.16*400 r 


u. 

o 


u 

a 

D 


6 . 87*400 


4.58*400 


2.29*400 


0.00*400 

0.0 


Figur 


a 4.98*400 

o 


u. 

o 


u 

a 

£ 3. 59. *00 

Z 

a 


2.20*400 


8. 05*-01 


-5. 86*— 01 
0 J 


Fi 


* 4 M 




MAGNITUDE OF MODE 


78 






POSITION (FEET) 


79 



... time (SECONDS) 

Figure 3. l-9a Point of Resolution Position 



TIME (SECONDS) 

Figure 3. l-9b Point of Resolution Position 
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Link 1. Mode 3 
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Link 1, Mode 4 
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4.0 Conclusions 


The flexibility of a robotic manipulator may cause large 
errors at the end-effector of the system. Dynamic simulation of 
these vibrations and deflections is necessary to study the 
control of the system so these vibrations may be eliminated, or 
reduced. A general modeling method has been presented which 
includes the dynamics due to gross motions of the base, motions 
of the Joints, and vibrations of the Joints and links of the 
system. The motions of the Joints and the base are coupled to 
the vibrations of the Joints and links. The vibrations may be 
modeled using lumped parameters, truncated mode summation, or a 
component mode synthesis method. 

This model has been implemented in a simulation package 
called VSim. The package was used to simulate a large 
space-based manipulator system. Both mode summation and lumped 
parameter techniques are used in the model. The response of the 
system was seen to be configuration dependent. Very slow 
frequencies were present is some configurations, and not in 
other configurations. The response of the system also was 
dependent on the type and frequency content of the disturbance 
applied to the system. It should be noted that the second 
mode seemed to respond much the same as the lowest mode. This 
indicates that the system response probably can be modeled 
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sufficiently by only the first mode. 

This simulation package can be easily modified to Include 

a control algorithm. Disturbances may be applied externally, or 
may be created due to the motion of the links. The number of 
flexibilities, links, and Joints which the simulation package 
may model is resticted only by the constraints imposed by the 
computational resource which is used to run the simulation. 


APPENDIX A 


Derivation of the Local Inertia Matrix 

The expression for the kinetic energy of the ith 
link or sub- link is ideally expressed in integral form as 

“i " 2 J p( V { &X,) } * { S(x t ) J dV . ( A— 1 ) 

The velocity, RCx^, can be written as the sum of the velocity 
of a reference point and the velocity relative to the reference 
point. The reference point is chosen to coincide with the local 
origin of the link. The equation of velocity is then 

“’RU) = ll, R(a t ) ♦ <1> d(x i ) + <1> o>(O i ) x {“’dU) + (1, x | 

- "H. * "VV {'“«, } (A-2) 

♦‘""(2,) » |"’d(£ i ) 1 * a, d(x ) 

where B(Qj) is the translational velocity of the local 
coordinate system, is the rotational velocity of the 

local coordinate system, “’dtx^ is the velocity of the point 
of interest relative to the local coordinate frame, and U) d(x ) 

+ is the distance from the local coordinate frame to the 

point of interest. For notational brevity, define 
(l) vi v i _ <i> <n 

-V ■ -i " • (A-3) 

Also, the pre-script n) will be dropped, noting that all vectors 
in this appendix will be referenced to the local frame. 
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Substituting the expression for R( ^ ) , the kinetic energy 
can be expressed as: 

KE, - i J PC*,) { IV 

V 

+ J p(x t ) | | x x t | dv 

V 

+ J p(xj | • | uCOj) x d(x t )j dV 

V 

+ J p(x t ) | E(O t )j • | d( £i )j dV 

V 

+ 2 I p( -l } { 2 ( V X - 1 } * { y(&i) X "J dV 

V 

♦ J p(x t ) | u(g t ) x xj • | U(0J X dix^j dV 

V 

+ J p(x t ) | yCOJ X xj • | dCx^j dV 

V 

♦ i J p(x t ) | y(g t ) X d(x k )j • | u(g,) X dtx^j dV 

V 

+ J p(Xj) | wtg^ X dCjt^j • | d(x t )| dV 

V 

* | j pc*,) {ic*,)} • {it*,)} a* 

V 


(A-4) 
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These integrals define the total kinetic energy of a link 
Including that due to gross motion, vibrations, and the couping 
between the gross motion and vibrations. Each Integral can be 
rewritten so that the velocities are factored out of the 
integral. The integrations are performed off-line to predict the 
inertia terms that are needed for the dynamic model. 


The first kinetic energy term. 


5 J P( V { * ( V} * { B ( V} dV 


= | {sea,)} J p(x, ) dv [ru)} 

= 5 {E(a,)} T m i {R(O i )}. 


( A-S] 


letting 


[ ^mj ~ m i 


[I], this kinetic energy term becomes: 


= i { e<2,)} T [ { 6(0,’} 


( A-6) 


This term is the kinetic energy of the link moving as a point 


mass. 
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The second kinetic energy term is 


| p(x t ) | * { "(a,) X xj dV + 

V 

= J pCx^ | BCO t )]> • | w(Oj) x cmj dV + 

V 

J p(x t ) | RC^)} • { "(a,) x | x t - cmj | dV 

V 

= | FUO^j • | w(O t ) x [ J Pis,) Sffi, dv] ] - 

^ J V 

| R(a,l} • | y(a,) X | p(x ( ) | x t - cmj dV | 

( A- 7 ) 

The second integration in this kinetic energy term is zero because 

f p(x ( ) | x t - cml dV = J p(x t ) x ( dV - J ptx^ cr^ dV 

V 1 1 1 J v V 

= m cm - m cm * 0 . (A-8) 

i — l i — i 

The first Integration can be written in more convenient 
notation, 

| ma^j • | “(a t ) x ( I p<5 . 1 — 1 } 

^ J V 

= J p(xj A • | B x C j- dV . 

V 


(A-9) 
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Noting that cyclic permutations of the vectors In the scalar 
triple product do not change the value of the result, 


= J p(x { ) B • | C x A | dV . 


(A-10) 


Expressing the vector cross product in skew-symmetric form, 


Jp<S,>B T 


tc] A dV . 


C A— 1 1 ) 


Substituting the original vector functions, and moving them 
outside of the integral, 


= | p(£ f ) | wCO^j ^ cmj | R(0. i ) J- dV . 

V 

= J p ( x. i ) dV ufOj ) }[ cm^ j ^ R( 0^ , 

V 

" { ^ ( v} m i[ ^i] { s ( v} • 


( A— 12 ) 


Defining £ 1^ j = cm^j , this kinetic energy term can be 


expressed as: 


“{s'V } 1 [ ^ ] T { 


( A-13 ) 


This is the additional kinetic energy of a point mass 
resulting from the choice of the reference point. 



The third kinetic energy term is a couping term caused by 


deflections. It is similar in form to the previous term. The 
derivation is the same until the original vectors are 
substituted. 

J p(x,) | ) | * | x d(x,)j dV 

V 

= J p(x t ) B T [C] A dV 

V 

= | p(x t ) | [ d ( ) | | RIQ^j dV . (A- 14) 

V 

Remembering that the deflections are the sum of the normal 
modes , 

m 

. Jp( Sl ) {»(0,)} T [' [«„[?/*,>] ] { ^0 ( )} dv 

V (A-1S) 

The velocities and generalized coordinates of vibration can be 

moved outside of the integral: 

m 

■ {s ( v} I ( q .j J vv] dv ] { a ( Q, >} • 

J = 1 v 

(A- 16) 

Defining the integral over the skew-symmetric form of the mode 
shape to be 

[ M dij ] = J T 5j(X|)] dV . ( A-17 ) 

V 

this kinetic energy coupling term can be rewritten as 
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The fourth kinetic energy term Is a coupling term 
between the translational velocity and the velocities of 
vibration. 


J p(x t ) | RCO^j • | dCx^j dV 

V 

- {e(2,)} T Jp's,) 


(A-19) 


Substituting the mode shapes and generalized velocities of 


vibrations into the expression, 

m 


{ B<Q,5} T J P'x, 1 ]} 

to J " 1 


dV 


( A-20 ) 


and taking the summation and the velocities of vibrations out of 


the integral, we obtain 

m 

iT r i 


= a { * ( V}{ l ^u[ I p( ~ 1 ) W dV )} 


( A-21 ) 


Defining the integral over the mode shape: 

* J vv] dV - (A ~ 22) 

V 

and arranging these terms into a matrix, 


[: 1 . [m 1 

M 1 • • • 1 

M 1 , 

( A-23 ) 

[ mqi J L -dll 

-di2 

-dim 

i J 

fourth kinetic energy 

term can be 

rewritten as 


■ { S(o -.’} T [ w ' 

# 

. 3. • 


(A-24) 


i 
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dV 


dV 


The fifth kinetic energy term is the rotational kinetic 
energy of a rigid link. 

i | p(x t ) | X x t j • | w(0 t ) X xj dV 

V 

= i | p(x t ) | BtOj) x cn^j • | ^(0^ x cmj 

V 

+ 2 I P( “l } { - ( V X ’ { " ( “ l) X 2(Xl) } 

V 

+ | y(g t ) x J p (x t ) £i x t ) dv} • | »%) X cmj 

The last integral is zero because, 

J p(x t ) Je(x t ) dV 

V 

= J p(Xj) { - si t } dV 

V 

= | p(x t ) x 4 dV - J p(x t ) Cffij dv 


( A-25 ) 


= m cm - m cm 
1 — t i ' 


cm * 0. 


(A-26) 


The other two integrals in this kinetic energy term may be 
expressed in a more convenient notation: 

U' t v{ AxB H AxB } dv - 
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Noting that the dot product is commutative, and the expression 
can be manipulated as a scalar triple product, 

* 2 J P( - 1 J | A x B J x A • B 


dV 


\ | p(*j ) A* BxjAxBldV 

V L * 

2 J P(Xj) A • |Jb • BjA - |b • a|b 


dV 


* 2 J A *[[ B * b ]a - b[b • aJ J dV 

V 

* \ J p(x,) a t |b t ba - bb t a] dV 

V J 

= 2 J P( -1 } AT [ BTB fI] - BB T ]a dV 

ft J ' 


( A— 28 ) 


where [I] is the identity matrix. 

Substituting the original vectors, the kinetic energy term can 
be expressed as 

2 J p( -i J { y ( V x Hjj * | «(0,) x x t | dV 

2 J p( -i } £ £1j T SSI, [I] ~ cm ( cm^j w(0 ( ) dV 

* 2 J p( -i } y ( ^ )T [ ) 1 1 ) - M(x) X(x ) T ] u(0 ) dV 

V 1 J ~ 1 


(A-29) 


Notice that the term in brackets involving X is the three 
dimensional vector form of the parallel axis theorem. 
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Removing terms which are constant with respect to the 
integration, the integral in the first term yields the mass of 
the link. 

* i m t cm^ sm i [I] - Sffij cm i T j (l) w(0 i ) 

+ i J p(Xj) [ *(x,) T - JfCXj) dV S? ( V* 

V (A-30) 

The first term is the kinetic energy due to the fact that the 
reference point was not the center of mass and the parallel axis 
theorem must be used to find the equivalent inertia at the 
reference point. The second expression is the rotational kinetic 
energy of an undeflected body about the center of mass. The 
integral in the expression is the definition of the rigid-body 
rotational inertia matrix for a link. The expression can be 
rewritten in terms of the the rigid-body rotational inertia 

matrix 

- j “‘tt/ [ ] s'V * k s'V' [ 'u.1 ] s'V ■ 

( A-31 ) 

Defining the inertia about the local origin as 
[ I LL i ] " [ I c»/cm i ] + [ I LL i ]’ 

this kinetic energy term can finally be written as 

<*(0/ [ i LS ] »%) 


1 

2 


(A-33) 
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The sixth kinetic energy term is similar in form to the 
fifth term. 

J | x x^j- • | x dlx^j- dV ( A-34) 

Proceeding with the derivation in a similar way, 

- J P(£,) | A x B | • | A x C | dV . 


= J p*^) A • • b|a - |c • aJbJ dV 


_ j 

C p(x t ) A T 

C T BA - BC T a] dV 


\ 

-j 

1 

[ p(x ( ) A T 

. J 

C T B[ I ] - BC T 1a dV 

( A-35 ) 


I 

* J 


where [ I ] 

is the 

identity matrix. 

Introducing the original 

vector functions, 



" J p( V 

d(x t ) T «(x i )[I] - ^(x^ dfx^j dV . 




( A-36 ) 

can 

be removed from the Integral, 

resulting in 


= J p(x 4 )^ d(x^ ) T Jffx^tl] - ^(x^ d ( x^ ) T 1 ufOJ . 

V 1 1 J 1 


( A-37 ) 
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The resulting expression Is the rotational kinetic energy due to 
the deflection of the body. Introducing the mode shapes, 
n r ^ 

• a(0 t ) T [‘q u | Pds,) !,(£,) «*,)m (*-38) 

J s ^ L 

- fix^) S^Cx^) dV -1^' 

the integral can be defined to be 

* 

[ ^Ld, J ] ' I “V V*/ ] dV ' 

(A-39) 

and the sixth kinetic energy term can be rewritten as 



( A-40 ) 
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The seventh kinetic energy term is the couping between 
the rigid body rotations, and the velocities of vibration. 

J x • { d(^)| dV . ( A-41 ) 

V * ± J 

Noting that cyclic permutations of the vectors in the scalar 
triple product does not change the value of the result, 

= J Pfx,) w(0 t ) * | Xj x d(x ) | dV . (A-42) 

V L J 

Expressing the vector cross product in skew-symmetric form, 


* J pfx^ | y(0j) )’ [ M { dC^ )|^ dV 

m 

- Jp<V {-(0.) } [«,] { »,««.' ]} 

V ' 

T “ 

' {"‘V } / p(s i > [ *,] W dV )} 


dV 


( A— 43 ) 


Defining the integral across the cross product as 


&ij * J dV • 

V 

and defining a matrix, 



(A-44) 


(A-45) 


this couping term can be rewritten as 

* { s, v} K ] { 3 ,} ■ 


(A-46) 
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The eighth kinetic energy term is the extra rotational 
kinetic energy due to the deflection of the link. 


i J p(x 4 ) | “(Oj) * | - ( -i 5 x dV ( A-47 ) 

V 

which is similar is form to the rigid-body kinetic energy term. 
The derivation is the same up to the point where the original 
term are introduced into the equation. 

= i J pCx^ A T [b T B[ I ] - BB T ]a dV (A-48) 

V 

Substituting the original vectors, the kinetic energy term can 
be expressed as 


i J p(^) d(x t ) T d ( Xj ) [ I ] - d(x t ) dCx^J 


dV. 
(A-49 


Introducing the mode shapes results in 


* *1 f j 

* j s‘V T l t q .» I p(£ .’ [ ?/*>’] [ w] 


[wf 


dv u(g t ) . 


( A-50 ) 
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Defining the Inertia term associated with the integral as 


[ ‘ddijJ * I P( V 

V 


; W ] T [ W ] 


r T 

r 

-|T 

5 (x ) 

5 (x 

) 

L -i _1 J 

L -k -i J 


dV , 


( A-51 ) 


the kinetic energy term becomes 


i! [W W] 1 {*‘V} 


( A-52 ) 
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The nineth kinetic energy term is 

J p(x t ) | u(g t ) x d(x t )| • | d(x t )j dV 

V 

= J p(Xj) | uCO^j • | d(x t ) X dCx^j dV 
V 

= | uCOjj • p(x t ) d(x t ) d(x t ) dvj 

L V 

•{s'V}- {Jp '*. 1 J.K |ji <s >’]] 

ra 

V *. 1 ] dv } 

m m 

v J \)*1 k= 1 w 


a (X ) dV ql ( A- 53 ) 

-k "I IkJ 


Defining the integral to be 

f p‘5, 1 [?,<*,>] W dV 


M~ 

-ddi Jk 


{ A-54) 


this kinetic energy term can be rewritten as 


m 

- { S'V}’ [ * 


I V q M~ I 

L M 1 J ~dd 


ddljl _ddl12 


( A-S5 ) 


m 

V q M- q 

L -ddllm I 

J»1 1 


lk 
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The tenth and final kinetic energy term is 


2 I p( V {“’&*!>} * { <l> d(s i )J dV . 


(A-56) 


Introducing the mode shapes, 


■ *f P ' s '’ (jit W )} • {£( V VV )} dv • 

(A-57) 

then taking the velocity of vibrations outside the integral, 
results in 


IB IB 

1 1 


* J" P<JS * > [ • (w) d » 


( A-58 ] 


Defining the generalized mass of vibrations. 


C.J. ■ f ■ (?,<*,>]" ' 

V 


( A-59 ) 


which forms the generalized translational mass matrix of 
vibrations, 1^ j, the kinetic energy term can be written as 



( A-60 ) 
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At this point it should be noted that this derivation was 
based on the translational mode shapes of the link. The kinetic 
energy contribution of the rotational modes can be obtained via 
a method analogous to that used for translational modes. 
Assuming that the local link rotational deflections are 
decoupled from the local link translational deflections, the 
rotational kinetic energy of the link can be expressed as: 

KE* M \ l I/V(Xj ) | j" * £ j - dV - ( A- 61 ) 

V 

The angular velocity of a reference point can be written as the 
sum of the velocity of a reference point and the velocity 
relative to the reference point. The reference point is chosen 
to coincide with the local origin of the link. The equation of 
velocity is then 


w(x ) ■ w(Q,j) + © t x ^ ) , 
and the rotational kinetic energy is 


( A-62) 


J I/VCx^ -j 

0,(0^ 

V 


J I /V ( x t ) • 

[ 

V 

V 

J I/V(x t ) • 

{i‘V 


( A-63 ) 



The first integration occured in the previous derivation, 


because it is a measure of the curl of the translational 
velocity field. The second integration can be expressed as: 

ID 

" J I/V( V { } {jl ( ^ij W )} dV 


T * 

{* ( v} 


( A-64) 


Defining the integral to be 


M* 

-Ldl 


. * f I/V(x ) 0 (x ) dV 
J J i E j ~i 


(A-65) 


and defining a matrix 


(i" ,] = f M* I M" I ... I m" 1 
Lql [ -Ldll -LdI2 -Ldi» J ’ 


( A-66 ) 


this couping term can be rewritten as 


■ {? ( V} [C ] { a.} • 


( A-67 ) 


The third, and final integration can be expressed as 


-If V*! 1 ]} ‘ {,1 (’ m .*- 1 *. 1 ]}‘ 

V J=1 

Taking the velocity of vibrations outside the integral, 

-ill - 


(A-68) 


J = 1 k* V 


(A-69) 


Next, defining the generalized mass of vibrations, 


I 


R 

qql Jk 



</*,>] • 


(A-70) 


which forms 
vibrations. 


the 



rotational generalized mass matrix of 
The final integration can be expressed as 


The kinetic energy can now be presented in terms of the 


new mass and inertia matricies. 


Defining: 


[ X ] 

- 

» i 

CL .J 
►— « 

1 1 

+ 

KJ 

and 

r. i 

L ^ J 

as 

I P 

qq i J 

+ 

Kl 


( A-72 ) 


(A-73) 


the total kinetic energy is 

I { R‘a,>} T [ ] { a(2,)} 

* {s ( v} [ L ] T {i‘v} 

♦{ K>} t ['-..]{ *>} 

* 2 { ’} T [ 'LL, ] {?<V} 

*«{*■} [ ] { a.} 

* <5(KE) + 5 2 (KE) 


where 3(KE) and 5 2 (KE) are the kinetic energy term which are 
functions of the link deflections, and are small. 
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This expression can be organized into the desired quadratic 
form. 


KE 

t 



(A-75) 


B(Oj ) 

T 

[ *“,] [ 

‘-J [ ^ J 


R(0 ) 

■ G>( 0 ^ ) 

* 

[«J[ 

X -J [ 

* 

u(0 ) ■ 

• 

Si 

V. * 


Kf[ 

X Lqj [ I ‘N‘] 


• 

Si 


( A-76 ) 


This local inertia matrix is used in the derivation of the 
equations of motion. In deriving these inertia terms, the reader 
must be cautious that none of the assumptions or restrictions of 
the vibrations model are violated. Due to the generality 
of this modeling method, these restrictions are not Inherently 
included as the equations were derived. Those unfamiliar with 
the restrictions which apply to models of vibrations in 
continuous systems are urged to refer to Thomson [23]. 


APPENDIX B 


Derivation of the Stiffness Matrix 

The expression of the potential energy due to elastic 
deflection of a system is given as: 


J ei/v(x 4 ) a 2 /ax 2 j “’dCx^ 

1 • d Z /dx*I (l) d(x ) \ 
J v J 

v 1 



PE = 

i 


(B-l ) 

In this case, the link is assumed to be long and thin. The 
equation for the local deflection is assumed to be the sum 
of the normal modes: 


d(x ) = 5* q (1, S 

1 M lj _ij 


( B-2) 


such that 


a 

PE , ■ s J EI/V( V l‘ ■ 

v J=1 u J 

j] dV - 

(B-3) 

The summation and the generalized coordinates of vibrations can 
be taken outside the integral, 


in a 

i i 


(B-4) 


PE = i y V q q 

1 2 M it ,j ,k 

J El /V(x t ) a 2 /3x 2 { m s i} } • a 2 /3x 2 | (,, s ik j dv 
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Now, define the integration to be the stiffness term relating 
the potential energy due to the deflections and q^. 

< )k * f ei/v( 5i ) aVl" 1 ?,,} • « 2/3 !!*{"’«, k } dv • 

V 

(B-5 ) 

analogously for the rotational deflections: 

K* Jk - f GJ/VI^) 3/ax l | (l ’« 1J } • 3/3 5,{ ln * lk } dV 

V 

(B-6) 

The potential energy cam now be expressed in quadratic form. 



where ^ Kj 


( B-8 ) 



APPENDIX C 


VSim: A Simulation Package 

The source code for the simulation resides on the Silicon 
Graphics 4-D computer system in the University of Texas 
Mechanical Systems Robotics Lab. It may be found in the 
following directory: 

/usr/peopl e/phi 1 ip/VIBES 

To perform a simulation, the following files must be altered 
for the system which is to be simulated. 

/usr/peopl e/phi 1 ip/VIBES/Robotdat . c 
and 

/usr/peop 1 e/phi 1 i p/VI BES/Mode 1 . c 

The first contains the mass, geometry, and stiffness data for 
the system to be simulated. The control model for the system 
must be included In the second file. It is suggested that the 
user be familiar with the screen editor Vi, which is the common 
editor in a Unix system. The user must also be able to program 
in the computer language "C" to implement a control model. The 
whole program must be compiled by the following command: 
cc -03 -Zg -o VSim VSim.c 

This compiles the program using the full optimization option. 


122 



To execute the simulation, type: 

VSim outfile 

The word "outfile" refers to the name of the file in which the 
output data will be found. It may be replaced by any other name, 
and may include a directory, using the common Unix format. The 
program will ask what the starting and ending times should be, 
and what the minimum, and maximum time steps should be. 
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•include 

"raath.h" 

•include 

"stdio.h" 

* include 

"Robotdat.c" 

•include 

"External. c" 

•include 

"Const . c" 

•include 

"Cnath.c" 

•include 

"Gear.c" 

•include 

"Geometry . c" 

•include 

"Kinematics. c 

•include 

"Istar. c" 

= include 

"Kstar. c" 

•include 

"Torqua. c" 

•include 

"Modal. c" 


rain (argc, argv) 

int argc? 
char *argv[ ] ? 


i° u b** andtima, hh , hhmin, hhmax, aps , printtima, oldprint ; 
-oubl* t.yra; tord.r; , sav«r 12 ; r 0 rd*r J , ymaxrord«r T ; 
r°r b T*-,*^# 0 ui? rd * r ’ ' pwtord,rI [° r d«rl , told,hhold,hhn«w, •««; 
--r r ' * L 3* tarc , maxdar, nq, nqold, navq, k, numdata ; 
r*LZ *dat_fila, *fop«n(); 


print? ( "The output tilanama is callad \s n \ n", * f argv* 1 ) ) ; 

print; ("starttine- ?' n") : 
scan; ( "*lf " , &t } ; 
print; ( "starttirne- \e n",t)r 
print; { "endtima- ?\n"); 
scant("Uf",iandtina) ; 

print; ("endtima- ta\n" , andtima j ; 
print; ("how many data point* ?'n") ■ 

scant ("%d" , tnumdata ) ; 

print; (" initial stap siza- ?\n H ) ; 

scan; ihh) ; 

P r int; ( " in ltial stap size— %a n",hh) ; 
print; ( "minimum stap size- ?'n")- 

scant ("%If",*hhmin) ; 

print; ( "minimum stap siza- %a\n" , hhmin) ; 
print; ("maximum stap siza- *>'n")* 
scan; ( "% 1 t" , 4 hhmax) ; 

pr int; ( "maximum stap siza- la\n" , hhmax) ? 

print; ( "arror constant- ?\n") ; 

scant ( "%lt H , &aps) ? 

print; ( "arror constant- %*\n",eps); 

dat^tila-topan ( * ( argv-f 1 ) , "v" } ; 
for ( i-0 ; icordar ,* i^*+) y [ 0 ] [ i ] -0 . ; 

mt-0 ; 

for ( i-0 ; i<order ; i+*) \ 
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ymax [ i ] ♦ 0 ; 

•rror[ i] *Q . 0; 

} 

jstart*0 ; 
maxder-6 : 

printtime- (•ndtirne-t)/nuindata, 
oldprint-t? 


maxder, nw. itold, ihhold, ihhnew, tnq, inqold, inewq, &k) , 


jstart-1? , „ 

fprintf (dat_f ile , "%d\n H , dof ) : 
while (t<endtiae 4* kflag>0) { 
printf ( "%d" , nq) ; 
if ( t> (oldprint+printtime) ) { 

oldprint-t; 

f or ( j-0 • j <dof t j*+) 'fprintf ( dat_f ile ,”V1.4e " , y ( 0 ] t j ] ) 
fprintf (dat_f ile, "\n") ; 

G«ar ( it . y , save . ihh , ihhmin , ihhnax , eps . »f . ymax . error , ikf laq . 
ij start , maxder , pw , t told , ihhold, ihhn.w, inq, inqold, inewq, Uc) . 


fclose ( dat_f ile ) ; 

printf ( M kf lag-td\n" , kf lag) 


e , s « orintf ( "Please include tn# nane of the output file 


M tiL p AG£ }s 

** P00 « Quality 
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*d«fin« ordar dof*2 

typadaf doubla Vector£3j, Matrix[ 3 ] [ 3 ] ; 
double sa[n], ca[n], *theta[n], cthetafn] ; 
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Const (G , Gp) 


* INITIALIZE THE JACOBIAN AND 
/ * COMPUTE CONSTANT VALUES IN 


HESSIAN MATRICIES TO ALL ZERO */ 
THE JACOBIAN AND HESSIAN MATRICIES 


V 


double G[n] [6] [dof] , Gp [ n ] [ 6 ] [dof ] ; 


extern^ Lnt^state_type ( n] , statajdir [dof 3 , state_pos(n] , mtfn], rar[n]; 

extern doubla^alpha{n] r theta[n}, sa[nj, ca(n], stheta[n], ctheta[n! ? 

* COMPUTE THE TRANSCENDENTAL FUNCTIONS OF THE CONSTANT ANGLE 
LINK PARAMETERS */ 
for ( i*0 ; i<n ; i++) I 

sa[ ij*sin< alpha [ i ] ) ; 
ca [ i } *co» (alpha [ i ] } ? 
stheta [ i ] -sin ( theta ( i ] ) ; 
ctheta ( i ] *cos ( theta ( i ] ) ; 


* INTITIALIZE THE JACOBIANS */ 
for (j*0 ;j<n; j ++) < 

f or ( k*0 ; k< 6 ; k+^) ( 

ffor(l*0:l<dofrl + *) 
f or ( 1*0 ; l<dof ; 1***^ ) 


ar j 3 [It] £13*0. ; 
Gp[j] [k] [l]-o. ; 


* ASSEMBLE ALL CONSTANT FLEX DIRECTION COSINES V 


; ='n ; 

for ' j*h-l: j<n; j*-) < 

l*state_pos( h 1 ^3 f lag ( h J + 1 : 
for ( k*l ; k< l^nt [ i 3 ;k++) 

Gp[ j ] [state_dir[ k] 3 [k]*l . ; 
for ( r k<state_pos (h+1 1 ;k+^) 

Gp( jT:State__dlr[k]^3 1 [k]»l. r 


/* translational flex */ 
/* rotational flex 


* ASSEMBLE ALL CONSTANT JOINT DIRECTION COSINES V 
for f j *h-l o>*0 ; j — ) < 

G* j ' * 5-3*state_typerh3 ] [state_pos[hi j-l. 0, 
Op ; j ; ; 5-3*state_type[h ] ] [ state_pos[ h ] 3*1.0; 


ASSEMBLE ALL CONSTANT JOINT FLEX DIRECTION COSINES * ' 

■ : f 1 ag •; h ; ) \ 

for( j-h-1; j>-0; j— ) i n . 

Gf j ] r 5-3 *state_type £ h ] \ , state_pos. h, 1 . - 1 . 0 . 
Gp( j ] ' 5-3*state type: h] ; £ state_pos ; h ; *1 ] -1 • 0 ; 


* ALL GROSS MOTION DIRECTION COSINES V 

if 1 free) t 


for < 1 *6 ; 1>0 ; 1--) 
for ( 1*6 ; 1>0 r 1 — ) 


Gtj](6-l][dof-l]*l. ; 

Gpf j ] [6-1] [dof-1 ] *1 • ; 


) 1 
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^define max_ele dof*2 

.*«****+**+*******•+**«*«*/ 

/* PRINT MATRIX TO SCREEN V 
**+***+•*++**+**+**+**+***/ 


PRINTM(M, i, j) 

double *M; 
mt i,j; 


:nt k,l,row; 

print; ( M \n” ) ; 
for fk»0 ;k<i ; k+-*) 

row«k* j ; 
print; ( "\n H ) ; 
if ( * (M+row+1 ) >-0) 

for ( 1-0 ; 1< j ? 1++) printf ( H %1.2e " , * (M+row+1 ) ) ; 

• is* 

for (l-0;i<j ; 1*^) printf ( n \l . 2a " , * (M+row+1) ) ; 
printf ( M \n M ) ; 

} 

print; ( M \n M ) ; 
return f 1 ) r 


*******************/ 

* MATRIX MULTIPLY V 


i, j,:.\k,i,0) 

icubie *M, *M, *0; 
mt i, 3 ,k,i: 


; r.t e , f , g ; 

-- 7 : 1 »k) return f -I); /* check to see if matncies are conformable */ 

for e-0 ;e< l ;e~- ) / * row counter for matrix M */ 

for t f«0; f<l ; f+* ) /* column counter for matrix N *' 

* '0+l*«-f ) ■o.o; 

for (g»0 ;g< j ;g-~j * (0*l*e-f ) «■» *f.M*j*e-g; * * ( H-l *g+f } ; 

» 

return ( 1 ) : 


************* 

* TRANSPOSE */ 

**«****•**•*•/ 

T f M, i, j,N) 
mt i,j; 
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double *M, *N> 


int fc,l; 

Cor (fc-0!k<i:K+-f) 

for(l-o:l<jrl*+) * (M+lc*j + l) < 

) 

return ( 1) > 


* SCALAR MULTIPLY^*/ 


SM ( M , i » j ,S,M) 
int i, j i 

double *N» Sf 


int lc,lJ 
Cor (lc»0 ?k<i 

Cor ( 1*0 ; 1< j :l~+) 

- S * *(M*K*j~U: 


return f 1 ) ; 


:?.CSS PRODUCTS •/ 


r rossx I t , r , G) 

::atr:x t,G; 

•■actor r; 

0 * *r: 2 ;-t' 2 ; [ 0 > *rti; : 
0 : *r'0'-tro' [ 0 ] * r { 2 I : 
c : *r ; i:-t;i; ;o:*r;o; ? 



Drossy ' t , r , 0) 

matrix t,G; 
"ector r ; 


s*o* ; i ; - - 
s : i; ;i;-t 
s;2 ; :i;-t 
return ( 1) 


i; ’ 

2 ; ; 
o; : 


l] *r [2 j -t[ 2 1 
1 ' 


[li # r( l] ; 
[ l] *r[ 2 1 f 
;ij*r[0i ; 


Crcssz f t . r , G) 


r.Votc ,3 

01 f UOR QUALITY 
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Matrix t,G; 
vector r? 


G C 0 ] [2]-tCl] [2]*r[2]-t(2] [2]*r[l] 
G C i I [2 ]*t[2 ] [2]*r[0 J-t[0] [2] *r£2] 
G[ 2 ] [21*t[0] [23*rCl]-tCl] [2]*r[0] 
raturn(l); 


* GENERATE A ROTATION MATRIX */ 


ROT f axis , radians r M) 

:nt axis; 
icubls radians; 
Matrix M; 


int i , j ; 

fori i» 0 ; i< 3 : i*+) 

for ( j-o : j < 3 ? j ) 

M r i ] r j ] « o . o ; 


-f '3X1S--2) 

M^O] ' 0 ; -cos ( radians) ; 
-[O' II]* -sin ( radians ) ; 
M : 1 ; : 0 ] « -M f 0 1 [ 1 ' ; 

M r 1 * : I T »M f 0 ■ 'O' ; 

m * 2 ; ; 2 ; * i * o ; 

».se :f'axis**l) 

M' o * ; o * -cos f radians) : 

- 7 0 T ■ 2 ; »s in { radians) ; 
m ; : ; ; : ; - i , o ; 
m; 2; ;o;» *m r o ; : 2 : ; 
m ; 2 ; ; 2 ; *m r 0 j ; 0 ) : 

e*se if r axis-*0) 

M ; 0 ) : 0 J - 1 . 0 ; 

1 1 [ 1 j *C0S ( radians) ; 

-sin ( radians) ; 
M'2 ; : 1 -M ( I ’ ' 2 ' ; 
v ' 2 ; ; 2 ; -M r 1 ; ; i ; ; * 


return r 1 ; ; 
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/** 


***/ 

/* 

MATRIX INVERSE 

V 

/* 

using 

V 

t * 

Gaussian- Jordan 

V 

/* 

Elimination 

V 

/** 


***/ 


GInversa(ti , a, nn) 

/* ti points to tin matrix to ba inverted */ 

/* a points to the inverted matrix V 
/ * nn is the dimansion of tha matrix */ 

double *a, *ti; 
int nn; 

int ipiv [max_ala ] , indxr (max_ele ] , indxc [ max_ele ] ; 
int i, j , k, irow, icol, 1, 11# row, col; 
doubla big,dum,pivinvr 

for ( i*0; i<nn ?!•*♦) { 
row*i*nn; 

for ( j-0? j<nn ; \ 

*(a^row+])» * ( tit* row*} } ; 

I 

) 

f or f i -0 ; j < nn r j ) * ( ip iv+ j ) -0 ; 

for ( i*0 ; i<nn; i-*-) \ 
big»0 . ; 

for ( j*0; j<nn;.j«"*-) < 

row* j *nn ; 

if (* ( ipiv+j ) ! *1) < 

for (k*0 ;k<nn;k*-) i 

if(«(ipiv*k) — 0) ' 

if { fabs(* (a-^row+k) j >-big) : 

big-fabs f * < a^-row+k) ) ; 
irow*j ; 
icol*k ; 

I 

) 

else if (*Upiv«-k) >1) ! 

printf { "singular matrix")* 
return ; 

> 


— * f ipiv-^icol ) ; 
row»nn* irow ; 
col*nn*icol ; 
l f ( irow 1 «icol ) i 

for f 1-0 rl<nn; W) { 

dum* *(a+row^l); 
♦(a^row+1)* *(a^col>l); 
* (a+col+1) *dum; 


Cf PVOR QUALITY 


132 


) 

*( indxr+i) -irow; 

* ( indxc+i) -icol ; 

if (* (a+col + icol) ~0) ( 

printf ("singular matrix"); 
ratum ; 

} 

pivinv-i./ *(a+col+icol) ; 

•(a^col^icol) -1 . f 

for ( 1-0 ;l<nn ;!•*■+) * (a+col+X) ■ *{a+col+l)*pivinv; 
for( 11-0 ; ll<nn; 11++) ( 
if(ll!-icol) ( 

row-nrt*ll ? 
col-nn*icol ; 
dum- * ( a+row+icol ) ; 

* (a+rov+icol) -0 . ; 

for ( 1-0; l<nn? 1++) *(a+row+l) — * (a^col-1) *dum; 


for ( 1-nn-l ; l>-0 ; 1 — ) i 

if (♦( indxr^i) !- *(indxc+l)} < 
for ( k-0 ; k<nn ; k++) { 
rov-nn*k; 

dum- •farrow-*- * ( indxr+1) ) ; 

# (a+row+ * ( indxr+1) ) - *(a+row+ *< indxc^l ) ) ; 
*(a+row+ * f indxc^l) ) -dum; 

) 

) 

rsturn ; 
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Geometry (phi , t , tl , r) 


GIVEN THE GEOMETRY STATES, CREATE THE TRANSFORMATION MATRICIES AND 

^TE^^eIS^' POSITION VECTOR r[h] . THE CONSTANT TRANS- 
FORMATION MATRIX t[h] , THE TRANSCENDENTALS OF ALPHA[n] AND THETA' n. 
IrEALREADY ASSIGNED VALUES BY THE CONSTANTS ROUTINE V 


double phi [do f ] ; 
Matrix tlfn] ,t[n] ; 
Vector r[n]; 


P ^°r'h\ I H THE E TRANSFORMATION FROM LOCAL SYSTEM x TO SYSTEM H 
t’-ni-V'S 1 : the LOCAL TRANSFORMATION MATRIX FROM X to x-1 WHEN 
“ '* 1 i>h, OR FROM i to U1 WHEN i<h. 
r r n‘'3' : THE POSITION VECTOR FROM ORIGIN H TO ORIGIN N 


* 

extern int state_type«; nj , state_posfn] 
extern double raodefdof ] , aa(n] , earn] , 
extern Vector L[nJ? 


statejdir [do£ ] , mtfn], mrrn], h; 
theta fn], sthetain], cthetafn]? 


in t i,j,k,l,n; 
double a,s,c; 

vector del [ n ] , rdel[n], rl,r2; 

Matrix tl; 

* THE ROTATION MATRIX ASSOCIATED WITH FRAME h IS THE IDENTITY MATRIX V 

fcr'i-0 ; i<2 ; i--) f 

forf j*0; j<3 ;j-*) \ 

tirh][i][j]-o. ; 

t ' h ][ i 1 • j ] *0 . ? 
i 

tl ;h; ; i; [ ii-i. ; 

t ; h ; ; i ] ; i ; * l . ; 


:?UTE LOCAL DEFLECTIONS OF EACH LINK */ 

>0 ; i<n; 1**) i 

j =*state_pos [ 1 ) ♦ j flag [ i ; ; 

* INITIALIZE THE DEFLECTIONS TO ZERO V 
for (k-0?k<3 :k++) ( 

deltij [k]-o? 
rdel [ i J [ k) »o ; 


/* SUM THE TRANSLATIONAL DEFLECTIONS OF LINK X */ 

/* V. DENOTES THE STATE POSITION ASSOCIATED WITH A DEFLEC*! 

f or° E X- j 1 k< j -rat [ i 1 t k +* ) del [ i ] [ state _dir f k ’ ] *-phx ' k’ -node •. k ] : 

* SUM THE ROTATIONAL DEFLECTIONS OF LINK l */ ^ T n‘i 

* k DENOTES THE STATE POSITION ASSOCIATED WITH A DEFLE Ot 
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OF LINK i */ 

for ( ; k< j+iat( i)+mr[ i) ;k++) rdal [ i ] [stata^dirtk] ] — phifk] *moda[k: ; 

* COMPUTE 3 X 3 ROTATION KATFICIES AND POSITION VECTORS FOR ALL > h */ 
f or ( i*h+l ; i<n; i++) ( 

j**tata_poa[ i] ; 

/* COMPUTE TRANSCENDENTAL FUNCTIONS OF JOINT i */ 

/•IF JOINT i IS ROTATIONAL, INCLUDE JOINT i MAGNITUDE 

AND DEFLECTION (thata[i] is tha constant joint rotation) */ 
if { ! stata_typa[ ij ) ( 

c*cos (phi [ j } -*-phi ( j + l ] * j f lag[ i ] "-thata [ i j ) ; 

s-sin(phi(j ] +phi ( j + 1 ] *j flag[ i j+thataf i i ) ; 

) 

* IF JOINT i IS TRANSLATIONAL, OR CONSTRAINED, LEAVE OUT 
JOINT i MAGNITUDE AND DEFLECTION, AND INCLUDE CONSTANT 
LINK i PARAMETER TRANS CEN DENTALS*/ 

alsa ( 

c*cthata[ i ] ; 

s»athata( i ] ; 

I 

* FORM LOCAL TRANSFORMATION MATRIX TO TRANSFORM A VECTOR FROM 
LOCAL FRAME i TO LOCAL FRAME i- 1 . THIS TRANSFORMATION IS A 
FUNCTION OF THE TRANSCENDENT A LS OF JOINT i AND ALPHA i-1, 

AND LINK i -1 DEFLECTIONS */ 

fc»i-l; 

tl ; i : ;o: :0]-c-rdal [ k[ [ 2 [ *cafk] *s-rdal [k; [ 1] *sa [k] *s; 
ti ; i ; ;o; [ l • ■ -a-rdal 'kj [ 2 ; *ca r k ; *c*rdai ; k ; r i ; * sa(k; *c; 
tl ; i ; [0] [ 2 ; -rdal (kj [ 2 ] *sa[ k] +rdal [ k; ' 1 ; *caf k] ; 
tl : i] : 1] [0] -rdal (kj [2 ] *c^ca[k] *s-rdai:k; ; 0 ; *safk^ *s; 
tl * i ; [ 1 ; [ 1] » -rdal [k] r 2 ) *s*ca r k] *c-rdal [ k; [ o ; *sar k] *c; 
tl[ii'l][2]« -sa(k)-rdal [kj f 0]*ca[k; ; 
tl[i] [2 J f 0 j« -rdal [k] [ 1 ) *c+rdal f k] [6 ] *ca [ k J *s*sa[ k[ *s ; 

-1 ' i] [2] 'lj-rdal [k] [ 1] *s*rdal [kj [0 ) *ca[kj *c+sarkj *c; 
tl [ ij [2? [2]» -rdaljk] [0] *sa[k] ■►ca [ k; ; 

* FORM THE TRANSFORMATION MATRIX TO TRANSFORM A VECTOR FROM 
LOCAL FRAME 1 to GLOBAL FRAME h */ 

mm ( t : i- 1 ; , 3 , 3 , tl r i ; , 3 , 3 , t [ i : ) ? 

* FORM THE LOCAL POSITION VECTOR FROM ORIGIN i-1 TO ORIGIN i 
THIS VECTOR IS A FUNCTION OF THE LENGTH OF LINK i-1, THE 
DEFLECTION OF LINK i- 1 , THE TRANSLATION OF JOINT i, AND THE 
TRANSLATIONAL DEFLECTION OF JOINT i. THIS LOCAL POSITION 
VECTOR IS IN THE i -1 LOCAL FRAME */ 

* IF JOINT i IS TRANLATIONAL, INCLUDE JOINT i MAGNITUDE AND 
DEFLECTION */ 

if t stata_typa( i ; ) t 

m-stata_po* [ i ] ; 

for (k- 0 ;k< 3 ;k++) 

nrk]»LCi-l] Ck]*dal[ i-l] [k] 

«-t [ i ] [kj [ 2 ] * ( phi ’n[ ♦phi [ n- 1 ; *3 flag [ i ; ; : 

1 

, * IF JOINT 1 IS ROTATIONAL, LEAVE OUT JOINT i MAGNITUDE AND 
DEFLECTION */ 
else { 

for ( k-0 ;k< 3 ;k+«*) rl(k]-L( i-1 ] [kj -del [i-i; \k) ; 
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/* CONVERT LOCAL POSITION VECTOR TO FRAME H V 

/^FORM THE POSITION VECTOR FROM ORIGIN H TO ORIGIN I V 
fordt-Ot k<3 :k~) r(iHlc]-r[i-l] [k]+r2[)c] s 

I 

. COMPUTE 3X3 ROTATION MATRICIES AND POSITION VECTORS FOR ALL < H */ 

or ( i-h ; i> 0 ; i++) { 

j»state_pos [ i] ; 

/* COMPUTE TRANSCENDENTAL FUNCTIONS OF JOINT i V 
/* IF JOINT i IS ROTATIONAL, INCLUDE JOINT l MAGNITUDE 

AND DEFLECTION <thata[i] is tha constant }oint rotation) / 

Lf ( . stat *-^P*p^| j { ^phi c j ^ 1 ] * j f lag c i ] +th«ta r i j ) ; 

s-sin (phi C j ] ♦phi [ j -**1 ] * j f lag [ i 1 +thata [ i ] ) ; 

/* IF JOINT i IS TRANSLATIONAL, LEAVE OUT JOINT i MAGNITUDE 
AND DEFLECTION, AND INCLUDE CONSTANT LINK i PARAMETER 
TRANSENDENTALS */ 

else ( 

c-cthata[ i ] 
s-sthata f i } ; 

) 

/* FORM LOCAL TRANSFORMATION MATRIX TO TRANFORM A VECTOR FROM 
' [oS“"tOLOCAL FRAME i. THIS TRANSFORMATION IS A 
FUNCTION OF THE TRANSCEJJDENTALS OF JOINT i AND ALPHA 1-L. 
AND LINK i-1 DEFLECTIONS */ 

'kVOI (0}-c-rdel[k] ' 2 ) *ca ( k ; *s+rdel ; k] [ 1 j *sa [ k \ *s ; 

-s-rdel f k] [ 2 ] *ca r k ) *c-rdel [ k] [l/sa[k, c, 

: k’ : 2 : : 0l*rdalf k] £ 2 ] *sa f k) -rdel f k] [ 1 ] *cark] ; 

: k : : 0 ' r i : -rdel rk f 2 ] *c+ca[ k; *s-rdel [k] [ 0 ’ *sa[ k, s ; 

' k : : 1 ’ ; l 1 * -rdel [ki ' 2 ] *s+ca[k] *c-rdel (k) [ 0 ] *saf k] *c; 
: k i: 2^lU -»arkl-rd«l[k3C0]*ca[k]; , 

; kl'li ? 21- -rdel [ k] [ 1 ] *c+rdel [ k] [ 0 } *ca f k] *s+sa .. k ; s, 

: k : ; 2 ; r 2 ’ -rdel (k j [ 1 ) *s+rdel [ k] [0 ] *ca r k] *c^sa [k . c; 
jr : -rdel [ k ] ( 0 ] *sa ( k ] *ca [ k) ; 


k- 

tl 

ti 

tl 


tl 

tl 


. FORM THE TRANSFORMATION MATRIX TO TRANSFORM A VECTOR FROM 
LOCAL FRAME i-l to GLOBAL FRAME h */ 

KM(t: i; , 3 , 3,tl{k] ,3,3,t[k]) ; 

* FORM THE LOCAL POSITION VECTOR FROM ORIGIN 1 TO ORIGIN i- 1 
THIS VECTOR IS A FUNCTION OF THE LENGTH OF LINK i-l. ;**E 
DEFLECTION OF LINK i-1, THE TRANSLATION OF JOINT 1, AND THE 
TRANSLATIONAL DEFLECTION OF JOINT l V 
/* IF JOINT 1 IS TRANLATIONAL, INCLUDE JOINT 
DEFLECTION V 
if t lstate_type( i] ) ( 

m*state_pos [ i ] ; 

for(k-0;k<3 ;k+>) 

rl[k]» -L[ i-1 ] [k] -dal [ i-1 ] C k j _ . 

1 -tr i 1 rkl r 2 1 * (phi l m] +-phT ni -1 ] *3 f lag . 


i magnitude and 


/* IF JOINT i IS ROTATIONAL, LEAVE OUT JOINT i MAGNITUDE AND 
DEFLECTION V 
else ■ 
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for(k-0 ; k<3 :k*+) rl[k]» -L[ i-X] [k]-d*l[i-i ] [k] ; 

/* CONVERT LOCAL POSITION VECTOR TO FRAME H »/ 
MM(t[i-l],3,3,rl,3,l,r2) ; 

/* FORM THE POSITION VECTOR FROM ORIGIN H TO ORIGIN i-1 */ 
for (k«0 ;k<3 ; k*+) r( i-1] [k]-r[ l] [k]+r2(k ] ; ■ 
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Kinenat ics ( t , r , G , Gp ) 

'• sr si 3Ktsr!Siss5iis c ^T,si;«»si=r5rr2irs^ ■' 


Matrix t[n] ; 

Vector r [ n] ? 

double G[n] [6] [dot] ,Gp(n] [6] [dot ] ? 


int i, j, 1. H» 
Matrix GPJKP^n] [n] , 


12 , 13; 

GPJKD ( n ] [ n ] , GPJK [n] [n] ; 


extern int state Jiypefn] . state.dir £dof ) , state_posf n] , 


mt(n], arin]; 


COMPUTE ALL CROSS PRODUCTS WHICH ARE ™ OTO^ie “ 

OF THE ROTATIONS ON THE TRANSLATIONS OF THE LOCAL FRAMES , 


* COMPUTE THE CROSS PRODUCTS ASSOCIATED WITH THE GROSS MOTION 
OF LINK h •/ 


:or( i-0 ; i<h; i** ) < 

Crossx(t(h ] t r[ i] ,GPJK[h] [ i , ) ; 
Crossy ( tf H] * r' i ] , GPJK£ h ] C i ] ) ’ 
Crossz ( trh] , r£ i] ,GPJK[h] [i] ) ‘ 


) 

for{ i-h^L ; i<n; i+-) ( 

Crcssx ( t [ h ) , r ' i ] , GPJK r h ] i „ ) 
Crossy (trh] , rf i ] , GPJK ' h ] .i; ) 
Crossz f t ' h ] , r ' i' , GPJK ' h , ' i ] ) 


:e cross 


PRODUCTS ASSOCIATED WITH JOINT MOTIONS AND VIBRATIONS */ 


n-l . 


* rrp ALL LINKS DEFINE THE PROXIMAL END OF LINK i TO 

COINCIDE WITH THE LOCAL REFERENCE FRAME i, AND THE DISTAL END 
OF THE LINK TO COINCIDE WITH LOCAL REFERENCE FRAME L*1 / 

* NOTE THAT FOR LINKS NUMBERED > h, THE MOTION OF LINK i 

-0 THE MOTION OF LOCAL FRAMES 1, 1*1. - * *. Jrr HF ioTIOV 

(WHICH is DEFINED AS THE MOTION OF FRAME 1*1 T . V TI 

r.r * DI'P* ij CONTRIBUTES TO THE MOTION OF LOCAL FRAMES 1*1. ^ * ' * 

THEREFORE THE CROSS PROOUCTS ASSOCIATED WIT JJ^ 1 J2I T ? tth T up 
TUT*. - r -v ▼ Trrpr>jT THAN THE CROSS PRODUCTS ASSOCIATED WI*H THE 
"I”. ILPi 0F R LINKi. THE CROSS PRODUCTS ASSOCIATED WITH JOINT 
MOTIOrVwiLL SE CALLED GPJKP, AND THE CROSS PRODUCTS ASSOCIATED *ITH 
VIBRATIONS WILL BE CALLED GPJKD. */ 

. . DENOTES THE JOINT VELOCITY OR LINK VIBRATIONS REFERENCED TO THE 
tth LOCAL FRAME */ 


for ( i-h; i< (n-l) ; i*- ) < 

/* - DENOTES THE LOCAL FRAME WHERE THE POSITION VECTOR TERMINATES */ 


for ( j 


i«-l ; j<»n; j**) I 


/* 


COMPUTE CROSS PRODUCT OF LOCAL LINK 
POSITION VECTORS ORIGINATING AT THE 


i JOINT AXIS AND 
PROXIMAL END OF LINK 
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i+l AND TERMINATING AT THE ORIGIN OF LINK j . THIS IS 
THE CROSS PRODUCT ASSOCIATED WITH JOINT MOTION 1 */ 

for(k- 0 ;k <3 ;k++) rl(k]-r[ j 3 £Jc] -r [ i] ( k] ; 

Cro**2(t[i] ,rl,GPJKP(i] [j ] ) ; 

) 

for( j-i+2 ? j<*n; j++) < 

/* COMPUTE CROSS PRODUCT OF LOCAL LINK i AXES AND 
POSITION VECTORS ORIGINATING AT THE DISTAL END OF 
LINK i>I AND TERMINATING AT THE ORIGIN OF LINK j . 

THIS IS THE CROSS PRODUCT ASSOCIATED WITH THE 
THE VIBRATIONS OF LINK i */ 

for ( k»0 ; k< 3 ; k++ ) rl[k]-r[ j] [k]-r[ ij [k] ? 

/* THE CROSS PRODUCT IN THE JOINT DIRECTION IS V 
for(k»0 ;k< 3 ;k++) 

GPJKD[ i ] [ j ] (k J [ 2 ) *GPJKP[ i J [ j ] [ k] C 2 ] -GPJKP[ i] [ i+1 ] [k] [ 2 ] ; 

/* THE CROSS PRODUCTS IN THE X AND Y DIRECTIONS ARE * / 

CroMx ( t [ i ) , rl , GPJKD [ i ][ j ] J ; 

Cro**y ( t [ i] , rl,GPJKD[ i] [ j 1 ) ; 


* ‘HE h JOINT AXIS IS A SPECIAL CASE , WHICH EFFECTS THE LINKS NUMBERED < h */ 

* THE CROSS PRODUCTS ASSOCIATED WITH THIS HAVE BEEN COMPUTED, ONLY THE SIGN ' 
MUST 3 E CHANGED WHEN THIS CROSS PRODUCT IS USED FOR JOINT h MOTION */ 

NOTE THAT FOR LINKS NUMBERED < h, THE MOTION OF JOINT 1 CONTRIBUTES 
TO THE MOTION OF LOCAL FRAMES i-1, ... 0 , AND THE VIBRATIONS OF LINK 1 

V.HICH IS DEFINED AS THE MOTION OF FRAME i+1 RELATIVE TO THE MOTION 
OF JOINT i) CONTRIBUTES TO THE MOTION OF LOCAL FRAMES i, i-1 0 

BUT. THE CROSS PRODUCTS ASSOCIATED WITH THE MOTION OF JOINT 1 
'..‘ILL 3 E THE SAME AS THE CROSS PRODUCTS ASSOCIATED WITH THE 
VIBRATIONS OF LINK 1, SINCE THEIR RELATIVE MOTIONS COINCIDE AT THE 
LOCAL ith COORDINATE FRAME. */ 


* : DENOTES THE JOINT VELOCITY OR LINK VIBRATIONS REFERENCED TO THE 
-or. LOCAL FRAME */ 

“or . - r. - L ; 1 > 0 ; 1 - - • < 

^ DENOTES THE LOCAL FRAME WHERE THE POSITION VECTOR TERMINATES */ 

for ( — ) { 

/* COMPUTE CROSS PRODUCT OF LOCAL LINK i AXES AND 
POSITION VECTORS ORIGINATING AT THE DISTAL END OF 
LINK i-1 AND TERMINATING AT THE ORIGIN OF LINK j. 

THIS IS THE CROSS PRODUCT ASSOCIATED WITH BOTH 
THE JOINT MOTION i AND THE VIBRATIONS OF LINK 1 */ 

for ( k»o ; k< 3 ;k++) rl[k]-r[i] [k]-r[ j ] [k: ; 

Cro*sx ( t [ i ] , rl , GPJK[ i ] (j][0i); 

Cros*y(t[i] , rl,GPJK(i] ' j) [lj) ; 

Crossz (t [ i] , rl , GPJK[ i ] [ j j [ 2 ] ) ; 
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assemble the jacobians */ 

ti,j“^^ I s^«5 E »sss^isSS»«| , ‘S|?gs E ”^srgj|S!^Kr 

tSSsJSional and rotational deflections of LINK h (EXCEPT the^ross 

PRODUCTS ASSOCIATED WITH THE ROTATIONS )^^NO DE MOTION ARE 

(EXCEPT THE CROSS PRODUCTS ASSOCIATED WITH A huiai TIME- 

“gSFiJS CHANGED^ IN^THIS^ROUTINE V 

* j DENOTES THE JACOBIAN FOR FRAME j V 

. i INDICATES THAT THE JOINT MOTION OR VIBRATION IS ASSOCIATED WITH LINK i V 

. te DENOTES THE STATE VECTOR POSITION OF JOINT MOTION. LINK VIBRATION. 
OR°GROSS MOTION. THE CONVENTION USED IS k EQUALS. 

if 1-0 (link 0) : 

stat:«_po*tOJ...»Mt._po*tm«W-n tionai d . fl#ctions of link 0 . 


.Mt._po.W011 • ...MM po.[»t(01*mr[0]-i]^_ a 






if i-l...n-l (all othar joints and links): 

stata oosril - motion of joint i. 

if j flag - 1 U* thara is flax in joint i) 

if a mt7i°>0^ ( if *thara * '• traislatiSnal flax in" link i) 

Uatalpos C i 1 *3 a t g ^’ ;ii;^ona!'d2f lirt Ions of ‘ lint 1 . 

ius: c j ^ ^ c 1 : ^ 1 : 1 : : 

SZ - P ' . rntational deflections of link 1. 


state__pos r dof ] -6 

state”? 0 * [dof ] -5 

state_pos [dof [ -4 
state_pos [ dof ] -3 
state_pos [dof ]-2 
state_pos [dof ’ -1 


gross translation of link h 
gross translation of link h 
gross translation of link h 
gross rotation of link h in 
gross rotation of link h in 
gross rotation of link h in 


in the x direction 
in the y direction 
in the 2 direction 
the x direction 
the y direction 
the z direction 


• • THE NON-CONSTANT JOINT DIRECTION COSINES V 
* i DENOTES JOINT NUMBER */ 

- *" h n-««a.posU); has fl£x include „ I(I TH£ JAC OBIAN v 

if(jflag[i’J 2 < is THE STATE posITION 0F x„E OF JOINT 1 FLEX V 
/« j DENOTES JACOBIAN OF LOCAL FRAME j V 
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for( j*i; j<n? j++) { 

/* JOINT POSITION PARTS */ 

/* 1 IS ZERO IF TRANSLATIONAL , OR THREE 
IF ROTATIONAL JOINT V 
1 • 3-3*stata typa(i); 

j] [1] till "~fc C t ] [ 0 } [ 2 ] ; 

<5Cj3Cl + l][113 - t[i](l][2]; 
ccjici*a][ii3 - tciic 2 ][ 2 ]f 
<5P[J]C1][U] - t[i][0][2j; 

GpCj ] tl^l] [U] - t [ i ] [ l ] [ 2 ] ; 

«Ptj ] (1*2) [11] - t [ i j [ 2 ] [ 2 ] ? 

/* JOINT FLEX PARTS */ 

GCjldlCiai - t[l] [0] [2] ; 

G[j] [l+l] (12) - t [ i ] [ 1 ] [ 2 ] ; 

G[j][l+2][12] - t[i][2][2]; 

GP(j]Cl][l21 - t[ i ] [ 0 ] [ 2 ] ; 

Gp[j][l+l ][123 - t [ i ] [ l ] [ 2 ] ; 

Cp(j 3 £1+2] [12] - t[i] [2] [2] ; 

) 

» 

/* If* THIS JOINT HAS NO FLEX, LEAVE IT OUT OF THE JACOBIAN */ 
• Isa ( 

/* .j DENOTES JACOBIAN OF LOCAL FRAME j V 

for'{ j*i ; j <n ; j++) < 

/* JOINT POSITION PARTS */ 

/• 1 IS ZERO IF TRACTS LATIONAL, OR THREE 
IF ROTATIONAL JOINT V 
1 - 3-3*stata typ«[i]; 
g C 3 ; : 1 3 [ 1 1 ; : i ; ^ o ] [ 2 j ? 

G[ j 3 [1*12 r 113 - t: i; ; i] [2 ] ; 

C[j::i-2J[U3 - t[ i: [2] r2 3 ; 

Gp [ j ] [ 1 3 [ 11 3 » t [ i ] [ o ] [ 2 3 ; 

Gpr j j [ i+i 3 [ 11 ] * i] : i; [2 j ; 
cp[j;[i+ 23 fii] - t[ i : c 2 3 ; a i ; 


ALL .‘JON -CONSTANT JOINT CROSS-PRODUCTS */ 
i DENOTES JOINT NUMBER */ 

' :*!i-l ; :<n-l ; i-~) j 

/* CHECK TO SEE IF THE JOINT IS ROTATIONAL V 
if ' ! stata_typa[ i ] ) ( 

1 1 • stata pos ( i ] ; 

/* IF THIS 'JOINT HAS FLEX, INCLUDE IT IN THE JACOBIAN * 
if fjflag[i)) i 
12 - 11 + 1 ; 

/* 3 DENOTES JACOBIAN OF LOCAL FRAME : */ 
for( j-i+i; j<n; j+-j \ 

/* JOINT POSITION PARTS */ 

G[ j 3 [OJ [11] « GPJKP f i ' ' j ] r 0 1 ' 2 ‘ ; 

Grjjrijrnj - gpjkp: 1’ : j 3 : i' : 2] ? 

G [ j I [ 2 ] [ l i 1 - GPJKPri; ■ j 3 r 2 i [ 2 ] ? 
GpCi3C0][ii: - gpjkp: i;i jj [ 6 ; 323 ; 

Gp[ j 3 [ 1 ] [ 11 J - GPJKPf i] C j 3 [ 1 • [2 • ? 

<3p[ j 3 [2] [11] - GPJKPti] [ j ’[2 1 '2 1 ; 

/* JOINT FLEX PARTS */ 

G[ j ) [0] [12) - GPJKP [i)Cj][0][2); 

G [ j 3 C 1 3 [ 12 ] - GPJKP[i3:j3:ii[2]; 

G[ j 3 [ 2 ] [ 12 ] - GPJKPfi] : j ] [2] : 2 ] ; 

Gp[ j } [ 0 ] [ 12 3 * GPJKP [i][jj r 0]'2]; 
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Gp[j]tl][12] 
Gp( j H 2 ] C 12 ] 


GPJKPCi] [j ] [lj [2] ; 
GPJKP [ i ] c j ] [21 [2] ? 


/* IF 

•1st 


THIS JOINT HAS NO FLEX, LEAVE IT OUT OF THE JACOBIAN 

/* j DENOTES JACOBIAN OF LOCAL FRAME j V 
for( j-i+l; j<n; j++) ( 

/* JOINT POSITION PARTS V 
G [ j ] [0] [11] - GPJKPCi][j}[0][23; 
GCj]Cin n l - GPJKPCi] [j ] [l] [2] ; 
G[j]C2]CU3 » GPJKP[i][j][2]r2] ? 

Gp C j ] [ 0 ] [ 1 1 ] - GPJKP [ i ] t j H 0 ] [ 2 ] ; 

GPC31C11I11] * gpjkp C i ] [ j ] C i ] [ 2 ] ; 

Gp[ j ] C 2 ] C LI ] - GPJKPCi] C j ] C 2 ] [ 2 ] ; 


) 


* GROSS MOTION CROSS-PRODUCTS V 

* j DENOTES JACOBIAN OF LOCAL FRAME j */ 

f(Cr««) { 

for ( r j <n ; j*-*) { 

/* x-DIRECTION */ 

Gf j][0]Cdof-31-GPJK[h] C j 3 CO] CO] ; 
G[ j ] [ 1 ] (dof-3 ] -GPJKfh] [ j ] [ l] [0] ; 
G[ j ] [2] [dof-3 ] »GPJK( h ] [j] [2] CO] ; 
Gp[ j] [0] [ dof -3 ] *GPJK ( h ] C j ] C 0 ] [ 0 ] ; 
Gpr j ] [ I] (dof-3 ]*GPJK[h] ' j][ij :o] ; 
Gp C j ^ ] 2 ] [dof -3 ] *GPJK [ h ] [ j ] ■ 2] CO] ? 
/* y-DIRECTION V 
G [ j 1 [ 0 ] (dof-2 ] »GPJK(h] C j ) [0] C I] ? 
gc j i ci] :dof-2i-GPjKrhi c j : : i: [i] ? 
GC j] [2] [dof-2]«GPJKCh]C j](2] [I ] * 
GpC j 1 C°] rdof-2 ] *GPJK[ h ] [j] ; 
Gp[ j] Cl] [dof-2]-GPJK[h] [j 1 [1] CD • 
Gp[ j] [2] [dof-2]-GPJK[h] [ j] [2] [1] ; 
/* z-DIRECTION V 
G[ j] [0] [dof-l]*GPJKCh][ j](0] [2] ; 
Gf j 1 C 1 J (dof-l]«GPJK(h] C j ; £ 1 ] : 2 ] ; 
G( j J ( 2 ] (dof-1 ) *GPJK[h] { j ] [ 2 1 C 2 ] ; 
Gp[ j 1 ro] [dof-1 ]*GPJK[h] [ j ] [0] [2] ; 
Gp r j i r i 1 (dof-1 ] *GPJK(h ] [ j } [ 1] [2] : 
Gp r j * : 2 1 rdof-l]*GPJK[h] [ j ] '2] [2] ; 


* -HE NON-CONSTANT FLEX DIRECTION COSINES */ 

* 1 denotes link associated with this flex v 
for * i*h-l ; i<n-lr i*-) ( 

Ll-stata_posri]+jflag( i]*l; 

12-ll-mtfi] ; 

13»L2+mr[ i j ; 

/•* j DENOTES JACOBIAN OF LOCAL FRAME 3 */ 

.orfj i 1^3 ^ demotes STATE POSITION OF TRANSLATIONAL LINK FLEX 
AND THE COLUMN OF THE JACOBIAN */ 
for (k-11 ;k<12 :k++) { 

Gp[ j ] (0] [k] - tc i] [0] [statejdir'k. ; ; 

Gp [ j ] C 1 1 Ck] - t ( i ] ( L ] [ state_d i r [ k , . ; 
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G P£j][2]£k] - t£ i] [2 ] £*tata_dir[ k] ] ; 

/* k DENOTES STATE POSITION OF ROTATIONAL LINK 
AND THE COLUMN OF THE JACOBIAN V 
for ( ;k<l3 ?k++) { 

<2pCj] [3] [k] - tci] CO] [atata dir[k]]J 
G P(j] Celtic] - t£i] [l] £»tat«~dir[kj ] , 

<3p C j ] C s ] C k } - t[i] [2] £atata~dir£k] ] , 


> 


FLEX 


* THE NON-CONSTANT FLEX CROSS PRODUCTS */ 

* i DENOTES LINK ASSOCIATED WITH THIS FLEX */ 
for f i-h; i<n -2 ; i^) < 

U-stata_poi { i ]♦!♦ j flag £i]+mt£i); 
I2»ll+nr£ 1) ; 

* j DENOTES JACOBIAN OF LOCAL FRAME j */ 
for ( j»1^2 ? j<n; j«-*) | 


/ * utnuits STATE POSITION OF ROTATIONAL LINK FLEX 

AND THE COLUMN OF THE JACOBIAN */ 

for(]t»ll;)c<12;]t+*) | 

0pCj][0][lcj • GPJKD(i][jJtO][stat« dir[lc] 1 
Sp 3 finit] - GPJKD( i] [ j ] [ 1 ] [stata - dir [kl i 
Gp[5 ! [2] tic] ■ GPJKD[ i] [ j ] [ 2 ] rstata - dir [ ki j 


* FOR j < h */ 

* THE NON-CONSTANT FLEX DIRECTION COSINES */ 

* l DENOTES LINK ASSOCIATED WITH THIS FLEX */ 

for' i»h-l ? i>»0; 1 — ) ( 

i l**tat«_poa [ i>j flag [ i ! — l ; 

12*1 1-rat r i ; ; 

12-12-rtr ; i ) ; 

* } DENOTES JACOBI All OF LOCAL FRAME j */ 
for ( j»l ; j >« 0 ; j --) ( 

/• k DENOTES STATE POSITION OF TRANSLATIONAL LINK FLEX 
AND THE COLUMN OF THE JACOBIAN */ 

for (k«U ;k <12 ;k++) ( 

G[j]£0][k; ■ -t[ 1] £o; [stata dir[k][ 

G i3l;l]CK] • -t[ i J [i; ;stata“dirfk; : 

G C j I C 2 ] C K ) - -t[ i ; [2 1 rstata dir'k’ 

J ' ‘ 

* k DENOTES STATE POSITION OF ROTATIONAL LINK FLEX 
AND THE COLUMN OF THE JACOBIAN */ 
for( ;k<-13 ;k«-«-) ( 

G Cj][3][kJ * -t £ i ] £ 0 ] ; stata_d i r r k ] 1 , 

G tj][4]£k] - -t£i]£i] rstate dir[k] ; 

G (j][5][k] ■ -t[ i] £ 2 ; [stata~dir [k] ] 


* THE NON -CONSTANT FLEX CROSS PRODUCTS */ 

* I DENOTES LINK ASSOCIATED WITH THIS FLEX V 

5r'i«h-i?i>o?i— ) { 

ll*stata_pos r i]+j flag£ i 3 ♦ i+mt [ i ] ; 

12-ll-rar £ i ; ; 

* j DENOTES JACOBIAN OF LOCAL FRAME j V 


ORIGINAL PAGi 

OF POOR QUAL 


143 


for(j-i-l;j>-Orj^s ( STATE POSITION OF ROTATIONAL LINK FLEX 
' AND THE COLUMN OF THE JACOBIAN */ 


for (k-U;k<12 :k++) 

Gij][on*i 

GCJlCUClt] 
at j ) 121 CK] 


GPJK [ i] 1 3 H 0 1 1 State dir , k . 
GPJKtilt jHU [state dir k - 
GPJK[ i ] ( j ] C 2 ] [state_dir(k] ] . 


) 


) 


, the NON-CONSTANT JOINT DIRECTION COSINES V 

, i denotes joint number V 

5r( ‘” h JOINT FLEX. INCLUDE IT IN THE JACOBIAN V 

if (jflag(i]> l _ 

/‘"j 1 DENOTES JACOBIAN OF LOCAL FRAME j V 

for ( i«i-l?j>*0 ; j — ) l 

U /* JOINT POSITION PARTS V 

/* 1 IS ZERO IF TRANSLATIONAL, OR THREE 
IF ROTATIONAL JOINT V 
L - 3 - 3 *stata typ«(i]; 

Gcjicucu; - 

G[ 11 [ 1*1] [U] - -t£ i] L li l 2 y 
Grj][ 1 ^ 2 ](ii] - -trij[2U2: : 

/■i JOINT FLEX PARTS V 

GtjiriMxz: - c i ] : o ; ; 2 : ; 

Gf :121 » -tr 1] :n [ 2 ] : 

G? j][l*2] • 12] - “t ] i ] [ 2 ] : 2 ] ; 

J 

ir THERE IS NO JOINT FLEX, LEAVE IT OUT OF THE JACOBIAN V 

eL3e /* j DENOTES JACOBIAN OF LOCAL FRAME j */ 

for f i*i-l .* j >“0 • 3 “•) ( 

U /*■ JOINT POSITION PARTS V 

/* l IS ZERO IF TRANSLATIONAL, OR THREE 
IF ROTATIONAL JOINT V 
1 . 3 - 3 *stat* type[ i] : 

- -t:i;:o][2;; 

or j t [ i+i] [ 11 ] * -tr i][i] 1 2 j ; 
g[ j] ;l^2] [11] - -t[i] £2] .2) ? 

) 


L NON -CONST ANT JOINT CROSS-PRODUCTS V 

denotes joint number v 

/* CHECK TO SEE IF THE JOINT IS ROTATIONAL *' 
if f lstat«_typ«[ i] ) f 

y #*IF a THERE* IF ^ JOINT FLEX. INCLUDE IT IN THE JACOBIAN 
If ( jflag[i] ) ( ^ 

/* j DENOTES JACOBIAN OF LOCAL FRAME j */ 

for M-i-1 ;j>"0 f j"") { 

/* JOINT POSITION PARTS V 

GT j ] [0 j [ 11] “ -GPJK r i][j][0][2.r 
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SIJ][1][U] - -GPJK[i] [ j] [l] [2] ; 

«tinanm - -GPJKiijtjnznz]: 

/• JOINT FLEX PASTS •/ 

G[j][0][12] - -GPJK(i] [j] [0] [2] 
G[jJ[l]C12] “ -CPJK(i] [j] [1] [2] 
G[ j ] C 2 ] [ 12 ] - -GPJK(l][j][2][2] 


) 


LEAVE IT OUT OF THE JACOBIAN * 


/* IF THERE IS NO JOINT FLEX 

• lift I 

/* j DEMOTES JACOBIAN OF LOCAL FRAME j * 
for( j-i-i; j>-0? j— ) { 

/* JOINT POSITION PARTS */ 

G[j][0]CU] - -GPJK[ i] [ j ] [ 0 ] [ 2 

G[jJtX][113 

G£ j J [2](U] 


V 


) 


-GPJK[i][j][l][2] 
-GPJK [i][j][2][2] 


I 


■h-I;i>0;i — ) < 

/* CHECK TO SEE IF THE JOINT IS ROTATIONAL */ 
i f ( stat«_typ« ( i ] ) ( 

U-itat«_po«[ i ] ; 

/♦IF THERE IS JOINT FLEX, INCLUDE IT IN THE JACOBIAN V 
if ( jflag[ ij) ( 

12-11+1; 

/* j DENOTES JACOBIAN OF LOCAL FRAME j * / 
fort j-i-X; j>«0; j — j < 

/* JOINT POSITION PARTS */ 

G;j):o::n: - GPJKCij'jMo 1 ':;, 
c: j ; :i: :n; - GPjKti] * j : ; ; 


/* IF 
else 


or j ; ;2] ; n; 

m 

GPJKri; ' j 

: ? 2 

' 2 : ; 

/* JOINT FLEX 

PARTS */ 



cr j ; ;o; '12' 

- 

GPJKr i i r i 

• ro 

'2 ■ ; 

G[ j 3 ; i : ; 12 ; 

- 

GPJK[ i ] [j 

; \ i 

' 2 \; 

Gm'2;;i2) 

i 

* 

gpjkt i; • j 

■ [2 

•; 2 : ; 

) 

THERE IS NO JOINT FLEX, 

LEAVE IT 

OUT 

OF T 

/* ] DENOTES JACOBIAN 

OF LOCAL 

FRAME 1 

for ( 3-1-I ; j >-0 ? j — ) 

1 




/* JOINT POSITION PARTS 

V 


G[j] :onii; 

9 

GPJK' 1 1 r j 

3 ro 

'2 * ; 

G[ j 3 r i ; r xi ] 

« 

GPJKr i 3 r j 

r i 

' 2 ’ ; 

g ; j r 2 : ' i i 1 

- 

GPJKr i; ; j 

: [2 

[ 2 ’: 


THE JACOBIAN */ 


• GROSS MOTION CROSS-PRODUCTS */ 

* j DENOTES JACOBIAN OF LOCAL FRAME 1 */ 

-f r free j : 

fort j»h-l ; j>-0 ; j — ) ( 

/* x-DIRECTION */ 

G[ j J[ 0 H dof-3 ] -GPJKfh J C j ] 1 0 ] [0]; 

g: j : ci; :dof-3}«GpjKth][ j] :i] [0] ? 

Gl j : -2; [dof-3 J «GPJK[ h ] [ j ] [21 [0] ; 
/* y-DIRECTION */ 

GC j ] ■ o; ;dof-2] -GPJKfh] [ j i r 0 1 U j ; 
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C[jHlJ[dof-2]«GPJK[h] [j] [1] [lj; 
G(j][2]Cdof-2]-GPJK[h][j] [2][1]; 
/* 2-DIRECTION */ 

G[j] [0] (dof-lJ-GPJKth} [j] [0] [2] ; 
C[ j][X] [dof-1 ] *GPJK[h} [ j ] [ 1] E 2 ] ? 
<=[ j]£2] [dof-l]-GPJK[h]Cj] [2] C 2 j ? 
) 


a 
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Istar ( istr , istrc,g, t) 


double istr[n*l] [dof] [dof] , istrc[n+l] [dof ] [dof ] ; 
double g[n][6J[dof], t[n][3][3]; 


int i, j , Jc, row, col, cl; 

double a [dof ] [dof] , b[dof)[dof] f c[dof][dof], al,bl,cl,dl; 


l DENOTES THE FRAME OF LINK (OR LOCAL REFERENCE) NUMBER */ 
* COMPUTE THE istar OF EACH LINK SEPARATELY, AND ADD THEM UP 
AT THE END OF THE ROUTINE TO FORM istar FOR THE SYSTEM */ 


for (i*0 ? i<n; i++> 


/* INITIALIZE THE istar MATRIX FOR LINK i */ 
f o r ( r ov» o ? row< do f ; r ow++ ) 

for ( col -0 ;col<dof ;col++) 

istr[ i ] [row} (col]-o. ; 


FIRST, FORM THE UPPER-RIGHT SYMMETRIC PARTS OF 

THE SUB-MATRICIES ON THE DIAGONAL OF THE LOCAL INERTIA 

MATRIX, THEN FORM THE LOWER-LEFT PARTS. 

NEXT FORM THE OFF DIAGONAL SUB-MATRICIES IN THE UPPER- 
RIGHT AREA OF THE LOCAL INERTIA MATRIX, AND TRANSPOSE 
THEM TO GET THE LOWER-LEFT PARTS. */ 


/* TRANSLATIONAL MASS */ 

* PRE-MULTIPLY THE TRANSLATIONAL JACOBIAN BY ITS TRANSPOSE 
AND MULTIPLY BY THE MASS */ 
for ( row-0 ; row<dof ; row*- ) 

for (col-0 ;col<dof ;col«—) ( 

al-m( i] * (g[ i; [0] [row]*g{i] [0] [col’ 

-q[i: Cl] [row}*gCiJf i][col] 
i ] [2] [row] *g[ i] * 2 ] rcoi; j ; 
istr[ i J [ row] [col ] *-al ; 

) 


* RIGID BODY ROTATION */ 

* ; P f ER ‘ RIGHT TERMS, TAKING ADVANTAGE OF SYMMETRY */ 


* COMPUTE 

INERTIA 

, IN 

' GLOBAL 

FRAME 

: */ 

f or ( co 1-0 ; 

col < 3 rcol*- 

) 1 


al 

- Ul r i] 

[01 

: o; *t r i • 

; coi ; 

;o: 


•IU'i] 

; 0] 


' col ; 

; i] 



[0] 

[2] *tr i; 

[ col ; 

; 2 ; 

bl 

- ill r i j 

[i] 

[0] *t[ i] 

[col] 

f 0] 


*Ill[i] 

[i] 

C l I *t [ i J 

[col] 

f li 


*ni:i] 

[i] 

[2j*t[i] 

[col ] 

[2] 

Cl’ 

- Ill [ i] 

[2] 


[col] 

[0] 


—11 1 C i] 

[2] 


[col] 

[1] 


-Ilirii 

[2] 

[ 2 ] *t [ i ] 

[col] 

[2] , 


for ( row-col ; row< 3 ; row++) { 

a [ rowi [ col ] » t [ i ] [ row] [ 0 l *al 
-tr i] [row] [ l] *bl 
-t[ i] [row] ' 2 ' *cl 
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a[col] [row]- a[row][col]; 

J 

/* PRE- AND POST- MULTIPLY THE INERTIA BY THE ROTATIONAL 
JACOBIAN */ 

for (col-0 ;col<dof ;col«-+) { 

al- a(0] [0]*g[i] [3] [col] 

^a(0] (I]*g[in4][col] 

*a [ 0 ] [ 2 ] *g [ i ][ 3 ] [ col ] ; 

bl- a[L]rO]*g[i]C3]Ccol] 

+a[ I] [ 1 J i] [4] [col] 

*a(I](2]*g(i][5] [col] ; 

cl- a[2] [0]*g(i] [3] [col] 

-a[2] [1] *g[ i] [4 ] [col] 

♦a[2][2]*g(l][5] [col] ; 

for ( row-0 : row<dof ; row++ ) ( 
dI*gC i] [3 ] [row] *al 
♦g[i] [4] [row] *bl 
+g[i][5][rov]*clr 

istr[ i] [ row] [col ] +-di ; 

) 

) 

* ADD THE LINK FLEXIBLILITY INERTIA TERMS TO istr V 
/* (NO TRANSFORMATIONS ARE NECESSARY) */ 
j»mt[ i]*mr[ i] ; 
k-state_pos[ 11*j £lag[ i]«*l; 
for f row-0 ; row< j ; row++) 

for (col-0 ;col<j rcol-^) 

istr [ i ] [ row+k ] [col-k ; +-Iqq[ i ] [-row i [ col ] ; 


* RIGID-BODY/ROTATIONAL COUPLING */ 

* AN OFF-DIAGONAL MATRIX V 

* COMPUTE INERTIA IN GLOBAL FRAME V 
f or f col-0 ;col<3 ;col*+) { 

al- Ini [i][0](0]*t[i] [col ] [ 0 ] 

-Ini [ i ] [01 (l]*t[i ■ [col ■ [ 1] 

-lnl.;i] [0] [2J*t[i] [col] [2] ; 

bl- ini [ i] [l] [0] *tr i ] [ col; [0] 

-Inir i] [ l] [ l] *t[ i] [col] [l] 

-Ial[i] El][2]*t[i] (col) [2] ; 

cl- Ial[i • [2] [0]*t[i] [col] [0] 

-Iml [i] [2] [ 1] *t[ i] [col] [1] 

-►Iml [ i ] [ 2 ] [ 2 ] *t[ i ] [col ] [2] ; 

f or ( row-col ; row< 3 ; row++ ) ( 

arrow] [col]- t( i] [rovr] [ 0 ] *ai 
+t[i] [row] [1] *bl 
*t[i] [row] [ 2 ] *cl ; 

a [ col ] [ row ] -a ( row ] [ col ] ; 
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) 

/+ POST MULTIPY BY THE ROTATIONAL JACOBIAN */ 
for ( row-0 ; row<3 ; row++) 

for ( col-0 ;col<dof ;col++) 

bCrow] [col]- a [row] [OJ*g[i] [3] [col] 

♦a [ row ][l]*g[i][4][ col ] 
♦•[row] [2]*g[i] [ 5 ] [col] ; 

/* PRE MULTIPLY BY THE TRANSLATIONAL JACOBIAN */ 
for ( row-o ; row<dof ; row++ ) 

for (col-0 ;col<dof ;col+-*-) 

c( row ] [col]- g[i] [0] [row)*b[0] [col] 
-g[l][l][row]*b[l][col] 

♦g[i] [2] [row]*b[2] [coll ; 

/* ADD THIS MATRIX AND IS TRANSPOSE TO istr */ 
for ( row-0 ; row<dof ; row++ ) 

for (col-0 ;col<dof ;col++) { 

i*tr[ i] [row] [col ]+-c[ row] [col ] ; 
istr[i] [col] [ rowj+-c[row] [col] ; 

I 

/* TRANSLATION/ FLEX COUPLING */ 

/* MULTIPLY THE INERTIA BY THE TRANSFORMATION MATRIX */ 

]-nt r 1 ] ♦ nr [ l ] ; 

for ( row-o r row< 3 ; row+-> ) 

for (col-0 ;col< j ;col^) 

a [row] [col]- t[i] [row] [0]*lnq[i] [0 1 'col 1 
-t[ i] [row] [i]*Imq[i • [1] [col] ' 
-t[ i] [row] [ 2 ] *Imq[ i ] [2] [col ; r 

* ‘MULTIPLY BY THE TRANSLATIONAL JACOBIAN */ 
f or ( row-0 ; row<dof r row*- ) 

for ( col-0 ;col<]j ;coi*-) 

b{ row ] [col ] - g[ i ] [ 0 ] [ row 1 *a .[ 0 ] [ col ; 

*g [ i ] ' 1 ] [ row i*a[i; [ col ; 
-g[i}[2]: row] *a[ 2 ] [col ; ; 

* ADD THIS MATRIX AND ITS TRANSPOSE TO istr */ 
K-statt_pos ; i [ flag[ i ] -1 ,* 

f or f row-0 : row<dof ; row-*— ) 

for fcol«fc?col<k*j ;col«—) { 

istrc[ i ] ; row; [col ; -«b [ row ; [ col-k] ; 
istrc [ i ] [ col ] [ row ] *-b [ row [ [ col-’< [ ; 


* ROTATION/ FLEX COUPLING */ 

* MULTIPLY THE INERTIA MATRIX BY THE TRANSFORMATION MATRIX 
: -nt [ i ] ♦nr [ i ] ; 

for ( row-0 ; row<3 ; row++) 

forfcol-0;col<j ;col*~) 

a[ row] [col]- t [ 1] [ row] [ 0 ] *Ilq[ i] [0 ] [col ’ 

♦t ( i ] [row] [l]*Ilq[i] r l] : col ; 
♦t[i] [row] [2] *Ilq[i] [2] [col] ; 

* MULTIPLY BY THE TRANSLATIONAL JACOBIAN */ 
for f row-0 ; row<dof ; row-*—) 

for { col-0 ; col <j ;col+-**} 

b[row] [col]- g[i][3] [row; *a[0] [col] 

*9 E i ] C 4 ] [ row ] *a [ 1 ] [coll 
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<hj[ i] [ 5 ] [ row] *a[2] fcol] ? 

/* ADD THIS MATRIX AND ITS TRANSPOSE TO istr V 
k-stat«_poa ( i ] + j f lag [ i ] + 1 ; 
for ( row*0 ; row<dof ;row++) 

for (col*k;col<k+j ;col^) { 

i*trc[ i j [row] [col ]+«b[row] [col-k] ; 
i*trc[ij [col] [row]+«b[row] [col-k; ; 

) 

) 

* ADD THE CONTRIBUTIONS OF EACH LINK TO FORM THE GROSS Istar */ 
or ( row*0 ; row<dof ; row++ ) 

for i col*0 ; pol<dof ; cal***-) { 

i*tr[n] [row] [col ] -0 . ; 
l*trc[n] [row] [col]-0. ; 
for( i»0 ; i<n? i++) 

i*tr[n] [row] [ col ] +«istr [ i ; [row] [col] ; 
iatrc[n] [row] [col ]+«istrc[ 1] [ row] [col ] ; 

) 


!2 
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Xstar ( kstr) 

doubl* kstr [dof ' (dof ] ; 


int i , k, 1 , row, col ; 

for ( i-O ; i<n; i++) { 

k-stat«_pos( i ] ♦j f lag( i ] ; 

if ( j flag[i]— 1) kstr(k] [ k )-K joint [ i] ? 

: 

i-mt [ i ] *nr( i ] ; 
if (11-0) 

for ( row-0 ,* rov<l ;row++) 

for (col-0; cold ;col+d 

kstr [ k+row] [k*col]-klink[ i] [row] [col] 

I 
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Torqua (tqua, load,G) 

doubla tqua[dof ) , load[n] C$1 *G[n] [6] [dof] # 


int i , j ‘ 
doubla a; 


f or ( i*Q r i<dof r ( 
a-0 . ; 

for ( j*0 r j <6 ; j+**') l 
a--load[n-l 

) 

tqu« r . i]-lmoda[i] *a; 

) 
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Model (hh, time , y , save) 

double *hh, *time, y[3] [order] , *iav«; 


double phi [dof ], disturb ; 

Matrix tl [ n ] , t[n] ; 
vector r [ n] ; 

double end load [ n ] [ 6 ] , load [ dof ] , j o in t load [dof ] , tque f dof] ? 

double G[n] [6] [dof] ,Gp[n] [6] [dof] ? 

double istr [n«*l] [dof] [dof] , istrinv[dof ] [dof] ; 

double kstr[dof] [dof] , istrc[n+l] [dof] [dof] ; 

double pstr [n+1 j [dof] [dof] [dof] ; 

:nt i,j; 


for ( i*0 ; i<6 ; 

for( j-0? j<n; j^f) ( 

endload [j ] [i]»0. ; 

} 

for ( i-0 ; i<dof ; ) phi[i]-y[0] [i] ; 

Const (G,Gp) ; 

Geometry ( phi , t , t 1 , r ) ; 

Kinematics (t, r,G f Gp) ; 

Istar ( istr, istrc.Gp, t) ; 

* Pstar (G, H, SkevG, pstrj ; */ 
r'star ( kstr) ; 

* APPLY END LOAD*/ 

*tme>.3 Li *tme<2.) endload: n-1 ;* O’ -1000 . ; 
Ttrque f tque , endload , Gp ) ; 

* INVERT THE INERTIA MATRIX */ 

: Inverse (&istr[n] [0j ro] , istrinv, dof ) ; 

* APPLY JO TNT TORQUES */ 

-f ' *ti.r.e<3 . ) { 

}ointload [ 0 ] *0 . ; 

]omtloadf 1 ]*0.; 
joint loadf 2 ] *0 . ; 

3 ointload f 3 ] *0 . ; 
jointload] 4 ] *0 . ; 

Z cintload r 5 ) *0 . : 


* COMPUTE LOAD VECTOR AND ASSEMBLE RIGID BODY INERTIA */ 

~r ' : i^dof ; i — ) >. 

load ; i ; -tque[ i >j omt load ■' l ; 
for ( }* 0 ; 3 <dof r j ! 

* a-0. ; 

f or ( k*0 ; j <dof ; j a+*y [ 0 ] ' dof *k ' *pstar [ j 1 r l ’ ‘ k ’ ; 

loadf i ] -*a*y( 0] [dof+j ] ; * 

load [ i ■ — kstr[i] [ j ] *y(0] r j ; ; 

loadr i ] -* ( alph* istr [ n ] : i j [ j ]+beta*fabs(kstr [ i 1 - r j ] ) ) *y ' 0 1 'dof* j f * 
loadf i } -*istrc[n] [ i ] [ j ] * y[ l ] [dof*j 1 / *hh; 

) 
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for(i-0; i<dof ;i++) *<*ava+i)-y[0] [i+dof] ; 
for ( ; i<ordar; i++) { 

*(sava+i)-0. ; 
for ( j-0; j<dof ; j++) 

* (*ava+i) +- ( istrinv( i-dof ] [ j ]*load[ j ] ) ; 

) 

/* APPLY BREAKS IF DESIRED V 
for ( i*l ; 1<7 ; i++) { 

if (braicasti] ) l 

/* SET VELOCITY - 0 */ 

* ( sava+stata_pos f i ] } *0 « 0 ; 

/* SET ACCELERATION - 0 */ 

* ( sava+dof +stata_pos [ i ] } -0 . 0 ; 


ORlGf'IAL PfiQE (3 

OF POOR QUALITY 
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